#include "zf_driver_file.h"
#include "zf_common_headfile.h"
#include "Image.h"
#include "math.h"

//辅助判断数据
float Left_Slope=0;//斜率
float Left_Intercept=0;//截距
float Right_Slope=0;//斜率
float Right_Intercept=0;//截距
extern uint8_t My_Image[70][155]; 
// #define USE_num	image_h*3	//定义找点的数组成员个数按理说300个点能放下，但是有些特殊情况确实难顶，多定义了一点
// #define image_h      UVC_HEIGHT
// #define image_w      UVC_WIDTH
// #define bin_jump_num	1         //跳过的点数
// #define border_max	    image_w-2 //边界最大值
// #define border_min	    1	      //边界最小值	
int my_abs(int value)
{
if(value>=0) return value;
else return -value;
}

int16 limit_a_b(int16 x, int a, int b)
{
    if(x<a) x = a;
    if(x>b) x = b;
    return x;
}

int16 limit1(int16 x, int16 y)
{
	if (x > y)             return y;
	else if (x < -y)       return -y;
	else                return x;
}
//图像添加黑框
void image_draw_rectan(uint8(*image)[image_w])
{
	uint8 i = 0;
	for (i = 0; i < image_h; i++)
	{
		image[i][0] = 0;
		image[i][1] = 0;
		image[i][image_w - 1] = 0;
		image[i][image_w - 2] = 0;

	}
	for (i = 0; i < image_w; i++)
	{
		image[0][i] = 0;
		image[1][i] = 0;
		image[image_h-1][i] = 0;

	}
}
uint8  Longest_Left_Wite_Line[2]; //左边最长白列
uint8  Longest_Right_Wite_Line[2];//右边最长白列
uint8  Wite_Line[image_w];          //每一行的长白列个数
//寻找赛道起点
uint8 start_point_l[2] = { 0 };   //左边起点的x，y值
uint8 start_point_r[2] = { 0 };   //右边起点的x，y值
uint8 get_start_point(uint8(*bin_image)[image_w], uint8 start_row_uint8)
{
    int start_row = start_row_uint8; // 转换为有符号整数
    uint8 l_found = 0, r_found = 0;
    int start_column = 15;
    int end_column = image_w - 15;
    // int mid_column = (start_column + end_column) / 2;
    for (int attempt = 0; attempt < 2; attempt++)
    {
        // 重置最长白列数据
        memset(Wite_Line, 0, sizeof(Wite_Line));
        Longest_Left_Wite_Line[0] = 0;
        Longest_Left_Wite_Line[1] = start_column;
        Longest_Right_Wite_Line[0] = 0;
        Longest_Right_Wite_Line[1] = end_column;
        // 计算每列白点数量
        for (int col = start_column; col <= end_column; col++)
        {
            uint8 count = 0;
            for (int row = start_row; row >= 0; row--)
            {
                if (bin_image[row][col] == 0) break;
                count++;
            }
            Wite_Line[col] = count;
        }
        // 寻找左边最长白列
        if(HuanDao_Stas==3 || HuanDao_Stas==4 || HuanDao_Stas==5)
        {
            end_column=image_w/2;
        }
        if(HuanDao_Stas==10 || HuanDao_Stas==11 || HuanDao_Stas==12)
        {
            start_column=image_w/2;
        }
        for (int col = end_column; col >= start_column; col--)
        {
            if (Wite_Line[col] > Longest_Left_Wite_Line[0])
            {
                Longest_Left_Wite_Line[0] = Wite_Line[col];
                Longest_Left_Wite_Line[1] = col;
            }
        }
        // // 寻找右边最长白列（右半部分）
        // for (int col = mid_column-20; col <= end_column; col++)
        // {
        //     if (Wite_Line[col] > Longest_Right_Wite_Line[0])
        //     {
        //         Longest_Right_Wite_Line[0] = Wite_Line[col];
        //         Longest_Right_Wite_Line[1] = col;
        //     }
        // }
        // 寻找左起点
        for (int col = Longest_Left_Wite_Line[1]; col >= border_min; col--)
        {
            if (bin_image[start_row][col] == 255 && (col == 0 || bin_image[start_row][col-1] == 0))
            {
                start_point_l[0] = col;
                start_point_l[1] = start_row;
                l_found = 1;
                break;
            }
        }
        // 寻找右起点
        for (int col = Longest_Left_Wite_Line[1]; col <= border_max; col++)
        {
            if (bin_image[start_row][col] == 255 && (col == image_w-1 || bin_image[start_row][col+1] == 0))
            {
                start_point_r[0] = col;
                start_point_r[1] = start_row;
                r_found = 1;
                break;
            }
        }
        if (l_found && r_found && start_point_r[0]>start_point_l[0]) return 1;
        // 上移一行，检查边界
        start_row--;
        if (start_row < 2) break; // 防止越界
    }
    return 0;
}
//最长白列寻找边线
uint8_t Longest_Right_Line[image_h];
uint8_t Longest_Left_Line[image_h];
void Find_Longest_Wite_Line(uint8(*bin_image)[image_w])
{
    int start_j = start_point_r[1]; // 起始行：右边起点的y坐标
    int end_j = start_j - Longest_Left_Wite_Line[0]; // 计算终止行
    end_j = (end_j < 0) ? 0 : end_j; // 确保不越界

    for (int j = start_j; j >= end_j; j--) // 从下往上遍历行
    {
        int found_right = 0;
        // 右边线：从中间向右搜索，假设中间为image_w/2
        for (int col = Longest_Left_Wite_Line[1]; col <= image_w - 3; col++)
        {
            if (bin_image[j][col] == 255 && bin_image[j][col + 1] == 0 && bin_image[j][col + 2] == 0)
            {
                Longest_Right_Line[j] = col;
                found_right = 1;
                break;
            }
        }
        if (!found_right)
        {
            Longest_Right_Line[j] = image_w - 1; // 默认右边界
        }

        int found_left = 0;
        // 左边线：从中间向左搜索
        for (int col = Longest_Left_Wite_Line[1]; col >= 2; col--)
        {
            if (bin_image[j][col] == 255 && bin_image[j][col - 1] == 0 && bin_image[j][col - 2] == 0)
            {
                Longest_Left_Line[j] = col;
                found_left = 1;
                break;
            }
        }
        if (!found_left)
        {
            Longest_Left_Line[j] = 0; // 默认左边界
        }
    }
}
//存放点的x，y坐标
uint16 points_l[(uint16)USE_num][2] = { {  0 } };//左线
uint16 points_r[(uint16)USE_num][2] = { {  0 } };//右线
uint16 dir_r[(uint16)USE_num] = { 0 };//用来存储右边生长方向
uint16 dir_l[(uint16)USE_num] = { 0 };//用来存储左边生长方向
uint16 data_stastics_l = 0;//统计左边找到点的个数
uint16 data_stastics_r = 0;//统计右边找到点的个数
uint8 hightest = 0;//最高点
//八临域寻迹
void search_l_r(uint16 break_flag, uint8(*image)[image_w], uint16 *l_stastic, uint16 *r_stastic, uint8 l_start_x, uint8 l_start_y, uint8 r_start_x, uint8 r_start_y, uint8*hightest)
{
	uint8 i = 0, j = 0;
	//左边变量
	uint8 search_filds_l[8][2] = { {  0 } };
	uint8 index_l = 0;
	uint8 temp_l[8][2] = { {  0 } };
	uint8 center_point_l[2] = {  0 };
	uint16 l_data_statics;//统计左边
	//定义八个邻域
	static int8 seeds_l[8][2] = { {0,  1},{-1,1},{-1,0},{-1,-1},{0,-1},{1,-1},{1,  0},{1, 1}, };
	//{-1,-1},{0,-1},{+1,-1},
	//{-1, 0},	     {+1, 0},
	//{-1,+1},{0,+1},{+1,+1},
	//这个是顺时针

	//右边变量
	uint8 search_filds_r[8][2] = { {  0 } };
	uint8 center_point_r[2] = { 0 };//中心坐标点
	uint8 index_r = 0;//索引下标
	uint8 temp_r[8][2] = { {  0 } };
	uint16 r_data_statics;//统计右边
	//定义八个邻域
	static int8 seeds_r[8][2] = { {0,  1},{1,1},{1,0}, {1,-1},{0,-1},{-1,-1}, {-1,  0},{-1, 1}, };
	//{-1,-1},{0,-1},{+1,-1},
	//{-1, 0},	     {+1, 0},
	//{-1,+1},{0,+1},{+1,+1},
	//这个是逆时针

	l_data_statics = *l_stastic;//统计找到了多少个点，方便后续把点全部画出来
	r_data_statics = *r_stastic;//统计找到了多少个点，方便后续把点全部画出来

	//第一次更新坐标点  将找到的起点值传进来
	center_point_l[0] = l_start_x;//x
	center_point_l[1] = l_start_y;//y
	center_point_r[0] = r_start_x;//x
	center_point_r[1] = r_start_y;//y
	//开启邻域循环
	while (break_flag--&& (l_data_statics < USE_num) && (r_data_statics < USE_num))
	{
		//左边
		for (i = 0; i < 8; i++)//传递8F坐标
		{
			search_filds_l[i][0] = center_point_l[0] + seeds_l[i][0];//x
			search_filds_l[i][1] = center_point_l[1] + seeds_l[i][1];//y
		}
		//中心坐标点填充到已经找到的点内
		points_l[l_data_statics][0] = center_point_l[0];//x
		points_l[l_data_statics][1] = center_point_l[1];//y
		l_data_statics++;//索引加一

		//右边
		for (i = 0; i < 8; i++)//传递8F坐标
		{
			search_filds_r[i][0] = center_point_r[0] + seeds_r[i][0];//x
			search_filds_r[i][1] = center_point_r[1] + seeds_r[i][1];//y
		}
		//中心坐标点填充到已经找到的点内
		points_r[r_data_statics][0] = center_point_r[0];//x
		points_r[r_data_statics][1] = center_point_r[1];//y

		index_l = 0;//先清零，后使用
		for (i = 0; i < 8; i++)
		{
			temp_l[i][0] = 0;//先清零，后使用
			temp_l[i][1] = 0;//先清零，后使用
		}

		//左边判断
		for (i = 0; i < 8; i++)
		{
			if (image[search_filds_l[i][1]][search_filds_l[i][0]] == 0
				&& image[search_filds_l[(i + 1) & 7][1]][search_filds_l[(i + 1) & 7][0]] == 255)
			{
				temp_l[index_l][0] = search_filds_l[(i)][0];
				temp_l[index_l][1] = search_filds_l[(i)][1];
				index_l++;
				dir_l[l_data_statics - 1] = (i);//记录生长方向
			}

			if (index_l)
			{
				//更新坐标点
				center_point_l[0] = temp_l[0][0];//x
				center_point_l[1] = temp_l[0][1];//y
				for (j = 0; j < index_l; j++)
				{
					if (center_point_l[1] > temp_l[j][1])
					{
						center_point_l[0] = temp_l[j][0];//x
						center_point_l[1] = temp_l[j][1];//y
					}
				}
			}
		}
		if ((points_r[r_data_statics][0]== points_r[r_data_statics-1][0]&& points_r[r_data_statics][0] == points_r[r_data_statics - 2][0]
			&& points_r[r_data_statics][1] == points_r[r_data_statics - 1][1] && points_r[r_data_statics][1] == points_r[r_data_statics - 2][1])
			||(points_l[l_data_statics-1][0] == points_l[l_data_statics - 2][0] && points_l[l_data_statics-1][0] == points_l[l_data_statics - 3][0]
				&& points_l[l_data_statics-1][1] == points_l[l_data_statics - 2][1] && points_l[l_data_statics-1][1] == points_l[l_data_statics - 3][1]))
		{
			//printf("三次进入同一个点，退出\n");
			break;
		}
		if (my_abs(points_r[r_data_statics][0] - points_l[l_data_statics - 1][0]) < 2
			&& my_abs(points_r[r_data_statics][1] - points_l[l_data_statics - 1][1] < 2)
			)
		{
			//printf("\n左右相遇退出\n");	
			*hightest = (points_r[r_data_statics][1] + points_l[l_data_statics - 1][1]) >> 1;//取出最高点
			//printf("\n在y=%d处退出\n",*hightest);
			break;
		}
		if ((points_r[r_data_statics][1] < points_l[l_data_statics - 1][1]))
		{
			// printf("\n如果左边比右边高了，左边等待右边\n");	
			continue;//如果左边比右边高了，左边等待右边
		}
		if (dir_l[l_data_statics - 1] == 7
			&& (points_r[r_data_statics][1] > points_l[l_data_statics - 1][1]))//左边比右边高且已经向下生长了
		{
			//printf("\n左边开始向下了，等待右边，等待中... \n");
			center_point_l[0] = points_l[l_data_statics - 1][0];//x
			center_point_l[1] = points_l[l_data_statics - 1][1];//y
			l_data_statics--;
		}
		r_data_statics++;//索引加一
		index_r = 0;//先清零，后使用
		for (i = 0; i < 8; i++)
		{
			temp_r[i][0] = 0;//先清零，后使用
			temp_r[i][1] = 0;//先清零，后使用
		}
		//右边判断
		for (i = 0; i < 8; i++)
		{
			if (image[search_filds_r[i][1]][search_filds_r[i][0]] == 0
				&& image[search_filds_r[(i + 1) & 7][1]][search_filds_r[(i + 1) & 7][0]] == 255)
			{
				temp_r[index_r][0] = search_filds_r[(i)][0];
				temp_r[index_r][1] = search_filds_r[(i)][1];
				index_r++;//索引加一
				dir_r[r_data_statics - 1] = (i);//记录生长方向
				//printf("dir[%d]:%d\n", r_data_statics - 1, dir_r[r_data_statics - 1]);
			}
			if (index_r)
			{
				//更新坐标点
				center_point_r[0] = temp_r[0][0];//x
				center_point_r[1] = temp_r[0][1];//y
				for (j = 0; j < index_r; j++)
				{
					if (center_point_r[1] > temp_r[j][1])
					{
						center_point_r[0] = temp_r[j][0];//x
						center_point_r[1] = temp_r[j][1];//y
					}
				}
			}
		}
	}
	//取出循环次数
	*l_stastic = l_data_statics;
	*r_stastic = r_data_statics;
}
uint8 l_border[image_h];//左线数组
uint8 r_border[image_h];//右线数组
uint8 center_line[image_h];//中线数组
//获取边线
void get_left(uint16 total_L)
{
	uint8 i = 0;
	uint16 j = 0;
	uint8 h = 0;
	//初始化
	for (i = 0;i<image_h;i++)
	{
		l_border[i] = border_min;
	}
	h = image_h - 2;
	//左边
	for (j = 0; j < total_L; j++)
	{
		//printf("%d\n", j);
		if (points_l[j][1] == h)
		{
			l_border[h] = points_l[j][0]+1;
		}
		else continue; //每行只取一个点，没到下一行就不记录
		h--;
		if (h == 0) 
		{
			break;//到最后一行退出
		}
	}
}
//获取边线
void get_right(uint16 total_R)
{
	uint8 i = 0;
	uint16 j = 0;
	uint8 h = 0;
	for (i = 0; i < image_h; i++)
	{
		r_border[i] = border_max;//右边线初始化放到最右边，左边线放到最左边，这样八邻域闭合区域外的中线就会在中间，不会干扰得到的数据
	}
	h = image_h - 2;
	//右边
	for (j = 0; j < total_R; j++)
	{
		if (points_r[j][1] == h)
		{
			r_border[h] = points_r[j][0] - 1;
		}
		else continue;//每行只取一个点，没到下一行就不记录
		h--;
		if (h == 0)break;//到最后一行退出
	}
}
/********************************************特殊点判断函数*********************************************************/
//将上拐点寻址范围减小到10以下，，将原来的下边界10的范围改为109，，下拐点生长方向改为1,2（未验证）
//寻找特殊点 ，，
uint8 Y_Border_Min=1;//限制拐点的上下范围
uint8 Y_Border_Max=79;
uint8 X_Border_Max=border_max;
uint8 X_Border_Min=border_min;
//变量定义
uint8 L_Up_Turn_Left_Point_1[2] = {0};      //存储上转左拐点的坐标
uint16 L_Up_Turn_Left_Point_Flag_1 = 0;      //上转左拐点存在标志位，找到时置1
uint16 L_Up_Turn_Left_Point_Position_1 = 0;  //记录位置，即是二维数组中的第几个点
void Get_L_Up_Turn_Left_Point_1(uint8 Grade)
{
    uint16 i = 0;
    L_Up_Turn_Left_Point_Flag_1 = 0;
    L_Up_Turn_Left_Point_1[0] = 0;
    L_Up_Turn_Left_Point_1[1] = 0;
    L_Up_Turn_Left_Point_Position_1 = 0;
//    L_Up_Turn_Left_Point_Angle_1 = 0;       //将各个参数清零
    switch(Grade)
    {
        case 1:     //严格判断
        {
            for (i = 4; i < (data_stastics_l - 4); i++)    //L_Statics 为左侧边线点个数
            {
                if((points_l[i][1] >= (Y_Border_Min + 2)) && (points_l[i][1] <= (Y_Border_Max - 2)))    //为存储左侧边线的二维数组，拐点必须满足不在于左右两侧边界上
                {
                    if ((dir_l[i - 2] == 5 || dir_l[i - 2] == 4) && (dir_l[i - 4] == 5 || dir_l[i - 4] == 4) &&
                    	(dir_l[i + 2] == 3) && (dir_l[i + 4] == 2) && (dir_l[i] == 3 || dir_l[i] == 2))    //L_Grow_Dir存储每个边线点的生长方向
                    {
                       
                        L_Up_Turn_Left_Point_1[0] = points_l[i][0];
                        L_Up_Turn_Left_Point_1[1] = points_l[i][1];
                        L_Up_Turn_Left_Point_Flag_1  = 1;
                        break;
                    }
                }
            }
            break;
        }
        case 2:     //中等判断
        {
            for (i = 4; i < (data_stastics_l - 4); i++)
            {
                if((points_l[i][1] >= (Y_Border_Min + 2)) && (points_l[i][1] <= (Y_Border_Max - 10)))
                {
                    if ((dir_l[i - 2] == 5 || dir_l[i - 2] == 4) && 
                        (dir_l[i - 4] == 5 || dir_l[i - 4] == 4) &&
                        (dir_l[i + 2] == 2 || dir_l[i + 2] == 1) && 
                        (dir_l[i + 4] == 2 || dir_l[i + 4] == 1) &&
                        (dir_l[i] == 3 || dir_l[i] == 4 || dir_l[i] == 5))
                    {
                       
                        L_Up_Turn_Left_Point_1[0] = points_l[i][0];
                        L_Up_Turn_Left_Point_1[1] = points_l[i][1];
                        L_Up_Turn_Left_Point_Flag_1  = 1;
                        break;
                    }
                }
            }
            break;
        }
        case 3:     //宽松判断
        {
            for (i = 4; i < (data_stastics_l - 4); i++)
            {
                if((points_l[i][1] >= (Y_Border_Min + 2)) && (points_l[i][1] <= (Y_Border_Max - 2)))
                {
                    if ((dir_l[i - 2] == 5 || dir_l[i - 2] == 4 || dir_l[i - 2] == 3) && 
                        (dir_l[i - 4] == 5 || dir_l[i - 4] == 4 || dir_l[i - 2] == 3) &&
                        (dir_l[i + 2] == 2 || dir_l[i + 2] == 1|| dir_l[i + 2] == 3) && 
                        (dir_l[i + 4] == 2 || dir_l[i + 4] == 1 || dir_l[i + 4] == 3) &&
                        (dir_l[i] == 3 || dir_l[i] == 2 || dir_l[i] == 1))
                    {
                       
                        L_Up_Turn_Left_Point_1[0] = points_l[i][0];
                        L_Up_Turn_Left_Point_1[1] = points_l[i][1];
                        L_Up_Turn_Left_Point_Flag_1  = 1;
                        break;
                    }
                }
            }
            break;
        }
        default:break;
    }
    if(L_Up_Turn_Left_Point_Flag_1 == 1 )
    {
        L_Up_Turn_Left_Point_Position_1 = i;    //记录拐点的位置
    }
}

uint8 L_Right_Turn_Up_Point_1[2] = {0};
uint16 L_Right_Turn_Up_Point_Flag_1 = 0;
uint16 L_Right_Turn_Up_Point_Position_1 = 0;
float L_Right_Turn_Up_Point_Angle_1 = 0;
void Get_L_Right_Turn_Up_Point_1(uint8 Grade)
{
    uint16 i = 0;
    L_Right_Turn_Up_Point_Flag_1 = 0;
    L_Right_Turn_Up_Point_1[0] = 0;
    L_Right_Turn_Up_Point_1[1] = 0;
    L_Right_Turn_Up_Point_Position_1 = 0;
    L_Right_Turn_Up_Point_Angle_1 = 0;
    switch(Grade)
    {
        case 1:
        {
            for (i = 5; i < (data_stastics_l - 4); i++)
            {
                if((points_l[i][1] >= (Y_Border_Min + 2)) && (points_l[i][1] <= (Y_Border_Max - 2)))
                {
                    if ((dir_l[i + 2] == 5 || dir_l[i + 2] == 4) && (dir_l[i + 5] == 5 || dir_l[i + 5] == 4) && (dir_l[i + 7] == 5 || dir_l[i + 7] == 4) &&
                        (dir_l[i - 2] == 6) && (dir_l[i - 5] == 6) && (dir_l[i] == 5 || dir_l[i] == 4))
                    {
                        L_Right_Turn_Up_Point_1[0] = points_l[i][0];
                        L_Right_Turn_Up_Point_1[1] = points_l[i][1];
                        L_Right_Turn_Up_Point_Flag_1  = 1;
                        break;
                    }
                }
            }
            break;
        }
        case 2:
        {
            for (i = 5; i < (data_stastics_l - 4); i++)
            {
                if((points_l[i][1] >= (Y_Border_Min + 10)) && (points_l[i][1] <= (Y_Border_Max - 2)))
                {
                    if ((dir_l[i + 2] == 5 || dir_l[i + 2] == 4) && 
                        (dir_l[i + 5] == 5 || dir_l[i + 5] == 4) &&
                        (dir_l[i + 7] == 5 || dir_l[i + 7] == 4) &&
                        (dir_l[i - 2] == 7 || dir_l[i - 2] == 6) && 
                        (dir_l[i - 5] == 7 || dir_l[i - 5] == 6) &&
                        (dir_l[i] == 4 || dir_r[i] == 5))
                    {
                        
                        L_Right_Turn_Up_Point_1[0] = points_l[i][0];
                        L_Right_Turn_Up_Point_1[1] = points_l[i][1];
                        L_Right_Turn_Up_Point_Flag_1  = 1;
                        break;
                    }
                }
            }
            break;
        }
        case 3:
        {
            for (i = 5; i < (data_stastics_l - 4); i++)
            {
                if((points_l[i][1] >= (Y_Border_Min + 2)) && (points_l[i][1] <= (Y_Border_Max - 2)))
                {
                    if ((dir_l[i + 2] == 5 || dir_l[i + 2] == 4 || dir_l[i + 2] == 3) && 
                        (dir_l[i + 5] == 5 || dir_l[i + 5] == 4 || dir_l[i + 5] == 3) &&
                        (dir_l[i + 7] == 5 || dir_l[i + 7] == 4 || dir_l[i + 7] == 3) &&
                        (dir_l[i - 2] == 7 || dir_l[i - 2] == 6 || dir_l[i - 2] == 5) && 
                        (dir_l[i - 5] == 7 || dir_l[i - 5] == 6 || dir_l[i - 5] == 5) &&
                        (dir_l[i] == 5 || dir_l[i] == 4))
                    {
                        
                        L_Right_Turn_Up_Point_1[0] = points_l[i][0];
                        L_Right_Turn_Up_Point_1[1] = points_l[i][1];
                        L_Right_Turn_Up_Point_Flag_1  = 1;
                        break;
                    }
                }
            }
            break;
        }
    }
    if(L_Right_Turn_Up_Point_Flag_1 == 1 )
    {
        L_Right_Turn_Up_Point_Position_1 = i;
    }
}

uint8 R_Up_Turn_Right_Point_1[2] = {0};
uint16 R_Up_Turn_Right_Point_Flag_1 = 0;
uint16 R_Up_Turn_Right_Point_Position_1 = 0;
float R_Up_Turn_Right_Point_Angle_1 = 0;
void Get_R_Up_Turn_Right_Point_1(uint8 Grade)
{
    uint16 i = 0;
    R_Up_Turn_Right_Point_Flag_1 = 0;
    R_Up_Turn_Right_Point_1[0] = 0;
    R_Up_Turn_Right_Point_1[1] = 0;
    R_Up_Turn_Right_Point_Position_1 = 0;
    R_Up_Turn_Right_Point_Angle_1 = 0;

    switch(Grade)
    {
        case 1:
        {
            for (i = 4; i < (data_stastics_r - 4); i++)
            {
                if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 2)))
                {
                    if ((dir_r[i - 2] == 5 || dir_r[i - 2] == 4) && 
                        (dir_r[i - 4] == 5 || dir_r[i - 4] == 4) &&
                        (dir_r[i + 2] == 2) && (dir_r[i + 4] == 2) && 
                        (dir_r[i] == 1 || dir_r[i] == 2 || dir_r[i] == 3))
                    {
                        R_Up_Turn_Right_Point_1[0] = points_r[i][0];
                        R_Up_Turn_Right_Point_1[1] = points_r[i][1];
                        R_Up_Turn_Right_Point_Flag_1 = 1;
                        break;
                    }
                }
            }
            break;
        }
        case 2:
        {
            for (i = 4; i < (data_stastics_r - 4); i++)
            {
                if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 10)))
                {
                    if ((dir_r[i - 2] == 5 || dir_r[i - 2] == 4) && 
                        (dir_r[i - 4] == 5 || dir_r[i - 4] == 4) &&
                        (dir_r[i + 2] == 1 || dir_r[i + 2] == 2) && 
                        (dir_r[i + 4] == 1 || dir_r[i + 4] == 2) &&
                        (dir_r[i] == 5 || dir_r[i] == 4 || dir_r[i] == 3))
                    {
                        
                        R_Up_Turn_Right_Point_1[0] = points_r[i][0];
                        R_Up_Turn_Right_Point_1[1] = points_r[i][1];
                        R_Up_Turn_Right_Point_Flag_1 = 1;
                        break;
                    }
                }
            }
            break;
        }
        case 3:
        {
            for (i = 4; i < (data_stastics_r - 4); i++)
            {
                if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 2)))
                {
                    if ((dir_r[i - 2] == -2 || dir_r[i - 2] == 1 || dir_r[i - 2] == 4) && 
                        (dir_r[i - 4] == -2 || dir_r[i - 4] == 1 || dir_r[i - 4] == 4) &&
                        (dir_r[i + 2] == 2 || dir_r[i + 2] == 3 || dir_r[i + 2] == 4) && 
                        (dir_r[i + 4] == 2 || dir_r[i + 4] == 3 || dir_r[i + 4] == 4) &&
                        (dir_r[i] == 2 || dir_r[i] == 3 || dir_r[i] == 4))
                    {
                        
                        R_Up_Turn_Right_Point_1[0] = points_r[i][0];
                        R_Up_Turn_Right_Point_1[1] = points_r[i][1];
                        R_Up_Turn_Right_Point_Flag_1 = 1;
                        break;
                    }
                }
            }
            break;
        }
    }
    if(R_Up_Turn_Right_Point_Flag_1 == 1)
    {
        R_Up_Turn_Right_Point_Position_1 = i;
    }
}

uint8 R_Left_Turn_Up_Point_1[2] = {0};
uint16 R_Left_Turn_Up_Point_Flag_1 = 0;
uint16 R_Left_Turn_Up_Point_Position_1 = 0;
float R_Left_Turn_Up_Point_Angle_1 = 0;
void Get_R_Left_Turn_Up_Point_1(uint8 Grade)
{
    uint16 i =0;
    R_Left_Turn_Up_Point_Flag_1 = 0;
    R_Left_Turn_Up_Point_1[0] = 0;
    R_Left_Turn_Up_Point_1[1] = 0;
    R_Left_Turn_Up_Point_Position_1 = 0;
    R_Left_Turn_Up_Point_Angle_1 = 0;
    switch(Grade)
    {
        case 1:
        {
            for (i = 5; i < (data_stastics_r - 4); i++)
            {
                if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 2)))
                {
                    if ((dir_r[i + 2] == 5 || dir_r[i + 2] == 4) && 
                        (dir_r[i + 5] == 5 || dir_r[i + 5] == 4) && 
                        (dir_r[i + 7] == 5|| dir_r[i + 7] == 4) &&
                        (dir_r[i - 2] == 6) && (dir_r[i - 5] == 6) && 
                        (dir_r[i] == 5 || dir_r[i] == 4))
                    {
                        R_Left_Turn_Up_Point_1[0] = points_r[i][0];
                        R_Left_Turn_Up_Point_1[1] = points_r[i][1];
                        R_Left_Turn_Up_Point_Flag_1 = 1;
                        break;
                    }
                }
            }
            break;
        }
        case 2:
        {
            for (i = 5; i < (data_stastics_r - 4); i++)
            {
                if((points_r[i][1] >= (Y_Border_Min + 10)) && (points_r[i][1] <= (Y_Border_Max - 2)))
                {
                    if ((dir_r[i + 2] == 5 || dir_r[i + 2] == 4) && 
                        (dir_r[i + 5] == 5 || dir_r[i + 5] == 4) && 
                        (dir_r[i + 7] == 5 || dir_r[i + 7] == 4) &&
                        (dir_r[i - 2] == 6 || dir_r[i - 2] == 7) && 
                        (dir_r[i - 5] == 6 || dir_r[i - 5] == 7) && 
                        (dir_r[i] == 4 || dir_r[i] == 5))
                    {
                        
                        R_Left_Turn_Up_Point_1[0] = points_r[i][0];
                        R_Left_Turn_Up_Point_1[1] = points_r[i][1];
                        R_Left_Turn_Up_Point_Flag_1 = 1;;
                        break;
                    }
                }
            }
            break;
        }
        case 3:
        {
            for (i = 5; i < (data_stastics_r - 4); i++)
            {
                if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 2)))
                {
                    if ((dir_r[i + 2] == -2 || dir_r[i + 2] == 1 || dir_r[i + 2] == 4) && 
                        (dir_r[i + 5] == -2 || dir_r[i + 5] == 1 || dir_r[i + 5] == 4) &&
                        (dir_r[i + 7] == -2 || dir_r[i + 7] == 1 || dir_r[i + 7] == 4) &&
                        (dir_r[i - 2] == -3 || dir_r[i - 2] == -4) && 
                        (dir_r[i - 5] == -3 || dir_r[i - 5] == -4) && 
                        (dir_r[i] == -2 || dir_r[i] == 1))
                    {
                        
                        R_Left_Turn_Up_Point_1[0] = points_r[i][0];
                        R_Left_Turn_Up_Point_1[1] = points_r[i][1];
                        R_Left_Turn_Up_Point_Flag_1 = 1;;
                        break;
                    }
                }
            }
            break;
        }
    }
    if(R_Left_Turn_Up_Point_Flag_1 == 1 )
    {
        R_Left_Turn_Up_Point_Position_1 = i;
    }
}

/**
* 函数功能：      寻找上转左拐点
* 特殊说明：      通过检测X值的跳变判定拐点的存在
*                 寻找方式为遍历一维数组边线的每个点
* 形  参：        uint8 Region_Start      起始行
*                 uint8 Region_End        截止行
*
* 示例：          Get_L_Up_Turn_Left_Point_2(2, 57);
* 返回值：        无
*/
uint8 L_Up_Turn_Left_Point_2[2] = {0};
uint8 L_Up_Turn_Left_Point_Flag_2 = 0;
uint8 L_Up_Turn_Left_Point_Position_2 = 0;
void Get_L_Up_Turn_Left_Point_2(uint8 Region_Start, uint8 Region_End)
{
    uint8 i = 0;
    L_Up_Turn_Left_Point_2[0] = 0;
    L_Up_Turn_Left_Point_2[1] = 0;
    L_Up_Turn_Left_Point_Position_2 = 0;
    if(Region_Start <= Region_End)
    {
        for(i = Region_Start; i <= Region_End; i++)
        {
            if(l_border[i] - l_border[i - 1] >= 5)
            {
                if(l_border[i + 1] - l_border[i - 2] >= 10)
                {
                    if(l_border[i] != 2)
                    {
                        L_Up_Turn_Left_Point_2[0] = l_border[i];
                        L_Up_Turn_Left_Point_2[1] = i;
                        L_Up_Turn_Left_Point_Flag_2 = 1;
                        L_Up_Turn_Left_Point_Position_2 = i;
                    }
                }
            }
        }
    }
}
/**
* 函数功能：      寻找右转上拐点
* 特殊说明：      通过检测X值的跳变判定拐点的存在
*                 寻找方式为遍历一维数组边线的每个点
* 形  参：        uint8 Region_Start      起始行
*                 uint8 Region_End        截止行
*
* 示例：          Get_L_Right_Turn_Up_Point_2(2, 57);
* 返回值：        无
*/
uint8 L_Right_Turn_Up_Point_2[2] = {0};
uint8 L_Right_Turn_Up_Point_Flag_2 = 0;
uint8 L_Right_Turn_Up_Point_Position_2 = 0;
void Get_L_Right_Turn_Up_Point_2(uint8 Region_Start, uint8 Region_End)
{
    uint8 i = 0;
    L_Right_Turn_Up_Point_2[0] = 0;
    L_Right_Turn_Up_Point_2[1] = 0;
    L_Right_Turn_Up_Point_Position_2 = 0;
    if(Region_Start <= Region_End)
    {
        for(i = Region_Start; i <= Region_End; i++)
        {
            if(l_border[i] - l_border[i + 1] >= 5)
            {
                if(l_border[i - 1] - l_border[i + 2] >= 6)
                {
                    if( l_border[i] != 2)
                    {
                        L_Right_Turn_Up_Point_2[0] = l_border[i];
                        L_Right_Turn_Up_Point_2[1] = i;
                        L_Right_Turn_Up_Point_Flag_2 = 1;
                        L_Right_Turn_Up_Point_Position_2 = i;
                    }
                }
            }
        }
    }
}
/**
* 函数功能：      寻找上转右拐点
* 特殊说明：      通过检测X值的跳变判定拐点的存在
*                 寻找方式为遍历一维数组边线的每个点
* 形  参：        uint8 Region_Start      起始行
*                 uint8 Region_End        截止行
*
* 示例：          Get_R_Up_Turn_Right_Point_2(2, 57);
* 返回值：        无
*/
uint8 R_Up_Turn_Right_Point_2[2] = {0};
uint8 R_Up_Turn_Right_Point_Flag_2 = 0;
uint8 R_Up_Turn_Right_Point_Position_2 = 0;
void Get_R_Up_Turn_Right_Point_2(uint8 Region_Start, uint8 Region_End)
{
    uint8 i = 0;
    R_Up_Turn_Right_Point_2[0] = 0;
    R_Up_Turn_Right_Point_2[1] = 0;
    R_Up_Turn_Right_Point_Position_2 = 0;
    if(Region_Start <= Region_End)
    {
        for(i = Region_Start; i <= Region_End; i++)
        {
            if(r_border[i] -r_border[i - 1] <= -5)
            {
                if(r_border[i + 1] - r_border[i - 2] <= -6)
                {
                    if(r_border[i] != border_max)
                    {
                        R_Up_Turn_Right_Point_2[0] = r_border[i];
                        R_Up_Turn_Right_Point_2[1] = i;
                        R_Up_Turn_Right_Point_Flag_2 = 1;
                        R_Up_Turn_Right_Point_Position_2  = i;
                    }
                }
            }
        }
    }
}
/**
* 函数功能：      寻找左转上拐点
* 特殊说明：      通过检测X值的跳变判定拐点的存在
*                 寻找方式为遍历一维数组边线的每个点
* 形  参：        uint8 Region_Start      起始行
*                 uint8 Region_End        截止行
*
* 示例：          Get_R_Left_Turn_Up_Point_2(2, 57);
* 返回值：        无
*/
uint8 R_Left_Turn_Up_Point_2[2] = {0};
uint8 R_Left_Turn_Up_Point_Flag_2 = 0;
uint8 R_Left_Turn_Up_Point_Position_2 = 0;
void Get_R_Left_Turn_Up_Point_2(uint8 Region_Start, uint8 Region_End)
{
    uint8 i = 0;
    if(Region_Start <= Region_End)
    {
        for(i = Region_Start; i <= Region_End; i++)
        {
            if(r_border[i] - r_border[i + 1] <= -5)
            {
                if(r_border[i - 1] - r_border[i + 2] <= -7)
                {
                    if(r_border[i] != border_max)
                    {
                        R_Left_Turn_Up_Point_2[0] = r_border[i];
                        R_Left_Turn_Up_Point_2[1] = i;
                        R_Left_Turn_Up_Point_Flag_2 = 1;
                        R_Left_Turn_Up_Point_Position_2 = i;
                    }
                }
            }
        }
    }
}
//组合寻拐点
uint8 L_Up_Turn_Left_Point[2] = {0};
uint8 L_Up_Turn_Left_Point_Flag = 0;
void Get_L_Up_Turn_Left_Point(void)
{
    L_Up_Turn_Left_Point_Flag = 0;
    Get_L_Up_Turn_Left_Point_1(2);
    if(L_Up_Turn_Left_Point_Flag_1 == 1)
    {
        L_Up_Turn_Left_Point_Flag_1 = 0;
        Get_L_Up_Turn_Left_Point_2(L_Up_Turn_Left_Point_1[1] - 7, L_Up_Turn_Left_Point_1[1] + 7);
        {
            if(L_Up_Turn_Left_Point_Flag_2 == 1)
            {
                L_Up_Turn_Left_Point_Flag_2 = 0;
                L_Up_Turn_Left_Point_Flag = 1;
                L_Up_Turn_Left_Point[0] = L_Up_Turn_Left_Point_1[0];
                L_Up_Turn_Left_Point[1] = L_Up_Turn_Left_Point_1[1];
            }
        }
    }
}
uint8 L_Right_Turn_Up_Point[2] = {0};
uint8 L_Right_Turn_Up_Point_Flag = 0;
void Get_L_Right_Turn_Up_Point(void)
{
    L_Right_Turn_Up_Point_Flag = 0;
    Get_L_Right_Turn_Up_Point_1(2);
    if(L_Right_Turn_Up_Point_Flag_1 == 1)
    {
        L_Right_Turn_Up_Point_Flag_1 = 0;
        Get_L_Right_Turn_Up_Point_2(L_Right_Turn_Up_Point_1[1] - 7, L_Right_Turn_Up_Point_1[1] + 7);
        {
            if(L_Right_Turn_Up_Point_Flag_2 == 1)
            {
                L_Right_Turn_Up_Point_Flag_2 = 0;
                L_Right_Turn_Up_Point_Flag = 1;
                L_Right_Turn_Up_Point[0] = L_Right_Turn_Up_Point_1[0];
                L_Right_Turn_Up_Point[1] = L_Right_Turn_Up_Point_1[1];
            }
        }
    }
}
uint8 R_Up_Turn_Right_Point[2] = {0};
uint8 R_Up_Turn_Right_Point_Flag = 0;
void Get_R_Up_Turn_Right_Point(void)
{
    R_Up_Turn_Right_Point_Flag = 0;
    Get_R_Up_Turn_Right_Point_1(2);
    if(R_Up_Turn_Right_Point_Flag_1 == 1)
    {
        R_Up_Turn_Right_Point_Flag_1 = 0;
        Get_R_Up_Turn_Right_Point_2(R_Up_Turn_Right_Point_1[1] - 7, R_Up_Turn_Right_Point_1[1] + 7);
        {
            if(R_Up_Turn_Right_Point_Flag_2 == 1)
            {
                R_Up_Turn_Right_Point_Flag_2 = 0;
                R_Up_Turn_Right_Point_Flag = 1;
                R_Up_Turn_Right_Point[0] = R_Up_Turn_Right_Point_1[0];
                R_Up_Turn_Right_Point[1] = R_Up_Turn_Right_Point_1[1];
            }
        }
    }
}
uint8 R_Left_Turn_Up_Point[2] = {0};
uint8 R_Left_Turn_Up_Point_Flag = 0;
void Get_R_Left_Turn_Up_Point(void)
{
    R_Left_Turn_Up_Point_Flag = 0;
    Get_R_Left_Turn_Up_Point_1(2);
    if(R_Left_Turn_Up_Point_Flag_1 == 1)
    {
        R_Left_Turn_Up_Point_Flag_1 = 0;
        Get_R_Left_Turn_Up_Point_2(R_Left_Turn_Up_Point_1[1] - 7, R_Left_Turn_Up_Point_1[1] + 7);
        {
            if(R_Left_Turn_Up_Point_Flag_2 == 1)
            {
                R_Left_Turn_Up_Point_Flag_2 = 0;
                R_Left_Turn_Up_Point_Flag = 1;
                R_Left_Turn_Up_Point[0] = R_Left_Turn_Up_Point_1[0];
                R_Left_Turn_Up_Point[1] = R_Left_Turn_Up_Point_1[1];
            }
        }
    }
}

//单调性检验，返回突出点（0无效）
int Left_Monotonicity_Change(int Start,int Stop,uint8 *Line)
{
    int i=0;
    int max=0;
    int Monotonicity_Point=0;
    //限制数组下标
    if(Start>=HEIGHT-5)
        {
            Start=HEIGHT-5;
        }
        else if(Start<5)
        {
            Start=5;
        }
        if(Stop>=HEIGHT-5)
        {
            Stop=HEIGHT-5;
        }
        else if(Stop<5)
        {
            Stop=5;
        }
        if(Start<Stop)
        {
            max=Stop;
            Stop=Start;
            Start=max;
        }
        //从下向上寻找
    for(i=Start;i>Stop;i--)
    {
        if(Line[i]==Line[i+5] && Line[i]==Line[i-5] &&
                Line[i]==Line[i+4] && Line[i]==Line[i-4] &&
                Line[i]==Line[i+3] && Line[i]==Line[i-3] &&
                Line[i]==Line[i+2] && Line[i]==Line[i-2] &&
                Line[i]==Line[i+1] && Line[i]==Line[i-1])//如果边线值相等视为没有找到突变点，无事发生
        {
            continue;//继续寻找
        }
        else if(Line[i] >Line[i+5]&&Line[i] >Line[i-5]&&
                Line[i] >Line[i+4]&&Line[i] >Line[i-4]&&
                Line[i]>=Line[i+3]&&Line[i]>=Line[i-3]&&
                Line[i]>=Line[i+2]&&Line[i]>=Line[i-2]&&
                Line[i]>=Line[i+1]&&Line[i]>=Line[i-1])//寻找十个边线里最突出的点，视为突变点
        {
            Monotonicity_Point=i;
            break;
        }
    }
    return Monotonicity_Point;
}

int Right_Monotonicity_Change(int Start,int Stop,uint8 *Line)
{
    int i=0;
    int max=0;
    int Monotonicity_Point=0;
    if(L_Border_Point_Num>0.9*HEIGHT && R_Border_Point_Num>0.9*HEIGHT)//丢线过多视为无效，结束函数
    {
        return Monotonicity_Point;
    }
    //限制数组下标
    if(Start>=HEIGHT-5)
        {
            Start=HEIGHT-5;
        }
        else if(Start<5)
        {
            Start=5;
        }
        if(Stop>=HEIGHT-5)
        {
            Stop=HEIGHT-5;
        }
        else if(Stop<5)
        {
            Stop=5;
        }
        if(Start<Stop)
        {
            max=Stop;
            Stop=Start;
            Start=max;
        }
        //从下向上寻找
    for(i=Start;i>Stop;i--)
    {
        if(Line[i]==Line[i+5] && Line[i]==Line[i-5] &&
                Line[i]==Line[i+4] && Line[i]==Line[i-4] &&
                Line[i]==Line[i+3] && Line[i]==Line[i-3] &&
                Line[i]==Line[i+2] && Line[i]==Line[i-2] &&
                Line[i]==Line[i+1] && Line[i]==Line[i-1])//如果边线值相等视为没有找到突变点，无事发生
        {
            continue;//继续寻找
        }
        else if(Line[i] <Line[i+5]&&Line[i] <Line[i-5]&&
                Line[i] <Line[i+4]&&Line[i] <Line[i-4]&&
                Line[i]<=Line[i+3]&&Line[i]<=Line[i-3]&&
                Line[i]<=Line[i+2]&&Line[i]<=Line[i-2]&&
                Line[i]<=Line[i+1]&&Line[i]<=Line[i-1])//寻找十个边线里最突出的点，视为突变点
        {
            Monotonicity_Point=i;
            break;
        }
    }
    return Monotonicity_Point;
}

//左边线圆弧（凹 凸）
uint8 L_Arc_Turn_Point[3][2] = {{0}};   //最多找三个就足够使用
uint16 L_Arc_Turn_Point_Flag = 0;    //找到一个就挂出标志位
uint8 L_Arc_Turn_Point_Num = 0;     //记录找到的个数
void Get_L_Arc_Turn_Point(void)
{
    uint16 i = 0;
    L_Arc_Turn_Point_Flag = 0;
    for(i = 0; i < 3; i++)      //每次调用先清零
    {
        L_Arc_Turn_Point_Num = 0;
        L_Arc_Turn_Point[i][0] = 0;
        L_Arc_Turn_Point[i][1] = 0;
    }
    for(i =7; i < (data_stastics_l - 5); i++)
    {
        //将圆弧拐点限制在有效范围内（排除上线边线附近的点，，减小误差）
        if((points_l[i][1] >= (Y_Border_Min + 2)) && (points_l[i][1] <= (Y_Border_Max - 7)))
        {
            if((points_l[i][0] >= (X_Border_Min)) && (points_l[i][0] <= (X_Border_Max)))//将圆弧拐点限制在有效范围内
            {
                if((points_l[i][0] > points_l[i - 7][0]) && (points_l[i][0] > points_l[i - 4][0]) && (points_l[i][0] >= points_l[i - 1][0]) &&
                    (points_l[i][0] >= points_l[i + 1][0]) && (points_l[i][0] > points_l[i + 3][0]) && (points_l[i][0] > points_l[i + 5][0]) &&
                    (dir_l[i + 1] == 3 || dir_l[i + 1] == 4) && (dir_l[i + 3] == 3 || dir_l[i + 3] == 4) &&
                    (dir_l[i - 1] == 4 || dir_l[i - 1] == 5 ) && (dir_l[i - 3] == 4 || dir_l[i - 3] == 5 ))                    //左侧外凸
                {
                    L_Arc_Turn_Point[L_Arc_Turn_Point_Num][0] = points_l[i][0];
                    L_Arc_Turn_Point[L_Arc_Turn_Point_Num][1] = points_l[i][1];
                    L_Arc_Turn_Point_Flag = 1;
                    L_Arc_Turn_Point_Num ++;
                    i += 15;    //每个圆弧拐点前后几个点可能都满足圆弧拐点的条件，所以找到后加15个点
                    if(L_Arc_Turn_Point_Num == 3)
                    {
                        break;
                    }
                }
            }
            
        }
    }
    for(i =7; i < (data_stastics_l - 5); i++)
    {
        if((points_l[i][1] >= (Y_Border_Min + 2)) && (points_l[i][1] <= (Y_Border_Max - 7)))
        {

            if((points_l[i][0] < points_l[i - 7][0]) && (points_l[i][0] < points_l[i - 4][0]) && (points_l[i][0] <= points_l[i - 1][0]) &&
               (points_l[i][0] <= points_l[i + 1][0]) && (points_l[i][0] < points_l[i + 3][0]) && (points_l[i][0] < points_l[i + 5][0]) &&
               (dir_l[i + 1] == 5 || dir_l[i + 1] == 4) && (dir_l[i + 3] == 5 || dir_l[i + 3] == 4) &&
               (dir_l[i - 1] == 4 || dir_l[i + 1] == 3) && (dir_l[i - 3] == 4 || dir_l[i - 3] == 3))                    //左侧外凸
            {
                L_Arc_Turn_Point[L_Arc_Turn_Point_Num][0] = points_l[i][0];
                L_Arc_Turn_Point[L_Arc_Turn_Point_Num][1] = points_l[i][1];
                L_Arc_Turn_Point_Flag = 1;
                L_Arc_Turn_Point_Num ++;
                i += 15;
                if(L_Arc_Turn_Point_Num == 3)
                {
                    break;
                }
            }
        }
    }
}
//右边线圆弧（凹 凸）
//寻找三个圆弧拐点（待定）
uint8 R_Arc_Turn_Point[3][2] = {{0}};
uint8 R_Arc_Turn_Point_Flag = 0;
uint8 R_Arc_Turn_Point_Num = 0;
void Get_R_Arc_Turn_Point(void)
{
    uint16 i = 0;
    R_Arc_Turn_Point_Flag = 0;
    for(i = 0; i < 3; i++)
    {
        R_Arc_Turn_Point_Num = 0;
        R_Arc_Turn_Point[i][0] = 0;
        R_Arc_Turn_Point[i][1] = 0;
    }

    for(i = 7; i < (data_stastics_r - 5); i++)
    {
        if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 2)))
        {
            if((points_r[i][0] < points_r[i - 7][0]) && (points_r[i][0] < points_r[i - 4][0]) && (points_r[i][0] <= points_r[i - 1][0]) &&
               (points_r[i][0] <= points_r[i + 1][0]) && (points_r[i][0] < points_r[i + 3][0]) && (points_r[i][0] < points_r[i + 5][0]) &&
               (dir_r[i + 1] == 3 || dir_r[i + 1] == 4) && (dir_r[i + 3] == 3 || dir_r[i + 3] == 4) &&
               (dir_r[i - 1] == 4 || dir_r[i + 1] == 5) && (dir_r[i - 3] == 4 || dir_r[i - 3] == 5))                    //右侧内凹
            {
                R_Arc_Turn_Point[R_Arc_Turn_Point_Num][0] = points_r[i][0];
                R_Arc_Turn_Point[R_Arc_Turn_Point_Num][1] = points_r[i][1];
                R_Arc_Turn_Point_Flag = 1;
                R_Arc_Turn_Point_Num ++;
                i += 15;
                if(R_Arc_Turn_Point_Num == 3)
                {
                    break;
                }
            }
        }
    }
    for(i = 7; i < (data_stastics_r - 5); i++)
    {
        if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 2)))
        {
            if((points_r[i][0] > points_r[i - 7][0]) && (points_r[i][0] > points_r[i - 4][0]) && (points_r[i][0] >= points_r[i - 1][0]) &&
               (points_r[i][0] >= points_r[i + 1][0]) && (points_r[i][0] > points_r[i + 3][0]) && (points_r[i][0] > points_r[i + 5][0]) &&
               (dir_r[i + 1] == 3 || dir_r[i + 1] == 4) && (dir_r[i + 3] == 3 || dir_r[i + 3] == 4) &&
               (dir_r[i - 1] == 4 || dir_r[i + 1] == 5) && (dir_r[i - 3] == 4 || dir_r[i - 3] == 5))                    //右侧外凸
            {
                R_Arc_Turn_Point[R_Arc_Turn_Point_Num][0] = points_r[i][0];
                R_Arc_Turn_Point[R_Arc_Turn_Point_Num][1] = points_r[i][1];
                R_Arc_Turn_Point_Flag = 1;
                R_Arc_Turn_Point_Num ++;
                i += 15;
                if(R_Arc_Turn_Point_Num == 3)
                {
                    break;
                }
            }
        }
    }
}

//检测左边丢线个数
uint8 R_Border_Point_Num = 0;   //记录位于左侧边界点个数
uint8 R_UP_Border_Point_Num = 0;////记录位于顶部黑框上的边界点个数
void Get_R_Border_Point_Num(void)
{
    int i = 0;
    R_Border_Point_Num = 0;
    R_UP_Border_Point_Num = 0;

    for(i = hightest; i < points_r[0][1]; i++)
    {
        if(r_border[i] == 152)    
        {
            R_Border_Point_Num ++;
        }
    }
    for(i = 80; i < data_stastics_r - 2; i++)
    {
        if(points_r[i][1] == 2)
        {
            R_UP_Border_Point_Num ++;
        }
    }
}
//检测右边丢线个数
uint8 L_Border_Point_Num = 0;       //记录左侧边界点个数
uint8 L_UP_Border_Point_Num = 0;    //记录位于顶部黑框上的边界点个数
void Get_L_Border_Point_Num(void)
{
    int i = 0;
    L_Border_Point_Num = 0;
    L_UP_Border_Point_Num = 0;
    for(i = hightest; i < points_l[0][1]; i++)
    {
        if(l_border[i] == 2)    //左右两侧用一维边线即可
        {
            L_Border_Point_Num ++;
        }
    }
    for(i = 40; i < data_stastics_l - 2; i++)
    {
        if(points_l[i][1] == 2)   //顶部的边界点使用二维边线
        {
            L_UP_Border_Point_Num ++;
        }
    }
}
//检测两边丢线个数
uint8 Relative_Border_Point_Num = 0;        //存储对位边界行的个数
void Get_Relative_Border_Point_Num(void)
{
    int i = 0;
    Relative_Border_Point_Num = 0;  //函数每张图像边线都调用一次，每调用一次都要清零
    for(i = hightest; i < start_point_l[1]; i++)
    {
        if(l_border[i] == 2 && r_border[i] == 152)   //2为左侧边界的X坐标，157为右侧边界
        {
            Relative_Border_Point_Num ++;
        }
    }
}
//检测两边丢线个数
int8 Row_Difference[20] = {0};
uint8 Max_Row_Dif_Line_Num = 0;     //当某一行左侧边线点位于左侧边界上，右侧边线点位于右侧边线上，与对位边界行同理，可以记录下来
void Get_Row_Difference(uint8 *L_border, uint8 *R_border)
{
    int i = 0;
    Max_Row_Dif_Line_Num = 0;
    for(i = 0; i < 20; i ++)
    {
        Row_Difference[i] = (int8)(R_border[i * 3] - L_border[i * 3]);
        if(Row_Difference[i] >= 154)
        {
            Max_Row_Dif_Line_Num ++;
        }
    }
}
uint8_t Left_Lose_One_Point=0;
void Find_OneLeftLose_Point()
{
    int i=0;
    Left_Lose_One_Point=0;
    for(i = hightest; i < start_point_l[1]; i++)
    {
        if(l_border[i] == 2 && r_border[i] != 152)   //2为左侧边界的X坐标，157为右侧边界
        {
            Left_Lose_One_Point++;
        }
    }
}
uint8_t Right_Lose_One_Point=0;
void Find_OneRightLose_Point()
{
    int i=0;
    Right_Lose_One_Point=0;
    for(i = hightest; i < start_point_l[1]; i++)
    {
        if(l_border[i] != 2 && r_border[i] == 152)   //2为左侧边界的X坐标，157为右侧边界
        {
            Right_Lose_One_Point++;
        }
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     左赛道连续性检测
  @param     起始点，终止点
  @return    连续返回0，不连续返回断线出行数
  Sample     continuity_change_flag=Continuity_Change_Right(int start,int end)
  @note      连续性的阈值设置为5，可更改
-------------------------------------------------------------------------------------------------------------------*/
int Continuity_Change_Left(int start,int end)
{
    int i;
    int t;
    int continuity_change_flag=0;
    if(Relative_Border_Point_Num>=0.9*HEIGHT)//大部分都丢线，没必要判断了
       return 1;
    if(start>=HEIGHT-5)
        start=HEIGHT-5;
    if(end<=5)
       end=5;
    if(start<end)//都是从下往上计算的，反了就互换一下
    {
       t=start;
       start=end;
       end=t;
    }
    for(i=start;i>=end;i--)
    {
        if(abs(l_border[i]-l_border[i-1])>=5)//连续性阈值是5，可更改
       {
            continuity_change_flag=i;
            break;
       }
    }
    return continuity_change_flag;
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     右赛道连续性检测
  @param     起始点，终止点
  @return    连续返回0，不连续返回断线出行数
  Sample     continuity_change_flag=Continuity_Change_Right(int start,int end)
  @note      连续性的阈值设置为5，可更改
-------------------------------------------------------------------------------------------------------------------*/
int Continuity_Change_Right(int start,int end)
{
    int i;
    int t;
    int continuity_change_flag=0;
    if(Relative_Border_Point_Num>=0.9*HEIGHT)//大部分都丢线，没必要判断了
       return 1;
    if(start>=HEIGHT-5)
        start=HEIGHT-5;
    if(end<=5)
       end=5;
    if(start<end)//都是从下往上计算的，反了就互换一下
    {
       t=start;
       start=end;
       end=t;
    }
    for(i=start;i>=end;i--)
    {
        if(abs(r_border[i]-r_border[i-1])>=5)//连续性阈值是5，可更改
       {
            continuity_change_flag=i;
            break;
       }
    }
    return continuity_change_flag;
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     左上角点检测
  @param     起始点，终止点
  @return    返回角点所在的行数，找不到返回0
  Sample     Find_Right_Down_Point(int start,int end);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Left_Up_Point(int start,int end,uint8 *Line)//找四个角点，返回值是角点所在的行数
{
    int i,t;
    int right_down_line=0;
    if(Relative_Border_Point_Num>=0.9*HEIGHT)//大部分都丢线，没有拐点判断的意义
        return right_down_line;
    // 调整start和end的顺序，确保start <= end（从上到下搜索）
    if (start > end) {
        t = start;
        start = end;
        end = t;
    }
    // 顶部5行不稳定，限制start最小值
    if (start < 5)
        start = 5;
    // 防止end超过有效范围（例如底部5行不稳定）
    if (end > HEIGHT - 5)
        end = HEIGHT - 5;
    // 确保i+3不越界
    if (end > HEIGHT - 4)
        end = HEIGHT - 4;
    // 根据hightest调整end范围（需根据实际变量含义调整）
    // if (end > HEIGHT - hightest)
    //     end = HEIGHT - hightest;
    // 从上到下遍历
   for (i = start; i <= end; i++) 
    {
        if(right_down_line==0&&//只找第一个符合条件的点
           abs(Line[i]-Line[i-1])<=5&&//角点的阈值可以更改
           abs(Line[i-1]-Line[i-2])<=5&&  
           abs(Line[i-2]-Line[i-3])<=5&&
              (Line[i]-Line[i+2])>=5&&
              (Line[i]-Line[i+3])>=10&&
              (Line[i]-Line[i+4])>=10)
        {
            right_down_line=i;//获取行数即可
            break;
        }
    }
    return right_down_line;
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     左下角点检测
  @param     起始点，终止点
  @return    返回角点所在的行数，找不到返回0
  Sample     Find_Right_Down_Point(int start,int end);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Left_Down_Point(int start,int end,uint8_t *Line)//找四个角点，返回值是角点所在的行数
{
    int i,t;
    int right_down_line=0;
    if(Relative_Border_Point_Num>=0.9*HEIGHT)//大部分都丢线，没有拐点判断的意义
        return right_down_line;
    //确保起始点start大于end。从下向上检测左下角点
    if(start<end)
    {
        t=start;
        start=end;
        end=t;
    }
    if(start>=HEIGHT-1-5)//下面5行数据不稳定，不能作为边界点来判断，舍弃
        start=HEIGHT-1-5;
    //确保数据的有效性
    // if(end<=HEIGHT-hightest)
    //     end=HEIGHT-hightest;
    if(end<=5)
       end=5;
    for(i=start;i>=end;i--)
    {
        if(right_down_line==0&&//只找第一个符合条件的点
           abs(Line[i]-Line[i+1])<=5&&//角点的阈值可以更改
           abs(Line[i+1]-Line[i+2])<=5&&  
           abs(Line[i+2]-Line[i+3])<=5&&
              (Line[i]-Line[i-3])>=5&&
              (Line[i]-Line[i-4])>=10&&
              (Line[i]-Line[i-5])>=10)
        {
            right_down_line=i;//获取行数即可
            break;
        }
    }
    return right_down_line;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右上角点检测
  @param     起始点，终止点
  @return    返回角点所在的行数，找不到返回0
  Sample     Find_Right_Up_Point(int start,int end);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Right_Up_Point(int start, int end,uint8_t *Line) // 找四个角点，返回值是角点所在的行数
{
    int i, t;
    int right_down_line = 0;
    if (Relative_Border_Point_Num >= 0.9 * HEIGHT) // 大部分都丢线，没有拐点判断的意义
        return right_down_line;
    // 调整start和end的顺序，确保start <= end（从上到下搜索）
    if (start > end) {
        t = start;
        start = end;
        end = t;
    }
    // 顶部5行不稳定，限制start最小值
    if (start < 5)
        start = 5;
    
    // 防止end超过有效范围（例如底部5行不稳定）
    if (end > start_point_l[1] - 5)
        end = start_point_l[1] - 5;
    
    // 确保i+3不越界
    if (end > HEIGHT - 4)
        end = HEIGHT - 4;
    // 根据hightest调整end范围（需根据实际变量含义调整）
    // if (end > HEIGHT - hightest)
    //     end = HEIGHT - hightest;
    // 从上到下遍历
    for (i = start; i <= end; i++) 
    {
        if (right_down_line == 0 && // 只找第一个符合条件的点
            abs(Line[i] - Line[i - 1]) <= 5 && // 检查下方连续行的稳定性
            abs(Line[i - 1] - Line[i - 2]) <= 5 &&
            abs(Line[i - 2] - Line[i - 3]) <= 5 &&
               (Line[i] - Line[i + 2]) <= -5 &&    // 对比上方行的突变
               (Line[i] - Line[i + 3]) <= -10 &&
               (Line[i] - Line[i + 4]) <= -10) {
            right_down_line = i; // 找到角点
            break;
        }
    }
    return right_down_line;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右下角点检测
  @param     起始点，终止点
  @return    返回角点所在的行数，找不到返回0
  Sample     Find_Right_Down_Point(int start,int end);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Right_Down_Point(int start,int end,uint8_t *Line)//找角点，返回值是角点所在的行数
{
    int i,t;
    int right_down_line=0;
    if(Relative_Border_Point_Num>=0.9*HEIGHT)//大部分都丢线，没有拐点判断的意义
        return right_down_line;
    //确保起始点start小于end。从下向上检测右下角点
    if(start<end)
    {
        t=start;
        start=end;
        end=t;
    }
    if(start>=HEIGHT-1-5)//下面5行数据不稳定，不能作为边界点来判断，舍弃
        start=HEIGHT-1-5;
    //确保数据的有效性
    // if(end<=HEIGHT-hightest)
    //     end=HEIGHT-hightest;
    if(end<=5)
       end=5;
    for(i=start;i>=end;i--)
    {
        if(right_down_line==0&&//只找第一个符合条件的点
           abs(Line[i]-Line[i+1])<=5&&//角点的阈值可以更改
           abs(Line[i+1]-Line[i+2])<=5&&  
           abs(Line[i+2]-Line[i+3])<=5&&
              (Line[i]-Line[i-2])<=-5&&
              (Line[i]-Line[i-3])<=-10&&
              (Line[i]-Line[i-4])<=-10)
        {
            right_down_line=i;//获取行数即可
            break;
        }
    }
    return right_down_line;
} 
//直道数据
// 12,15,17,
// 19,21,23,25,26,28,30,32,34,36,
// 38,40,43,46,48,50,52,54,56,58,
// 60,62,64,66,68,70,72,74,77,79,
// 82,84,86,88,90,92,94,96,98,99,
// 101,103,105,107,109,111,113,115,117,119,
// 121,123,126,128,130,132,134,135,137,139
uint8_t Zhi_Dao_BuChang[70]={ 
                            11,13,15,17,18,19,21,12,15,17,
                            19,21,23,25,26,28,30,32,34,36,
                            38,40,43,46,48,50,52,54,56,58,
                            60,62,64,66,68,70,72,74,77,79,
                            82,84,86,88,90,92,94,96,98,99,
                            101,103,105,107,109,111,113,115,117,119,
                            121,123,126,128,130,132,134,135,137,139
};
//计算赛道宽度
uint8_t Zhi_Dao_KuanDu[image_h];
void Get_SaiDao_Whith()
{
    int i=0;
    for(i=hightest;i<79;i++)
    {
        if(r_border[i]==152 && l_border[i]==2)
        {
            break;
        }
        else
        {
            Zhi_Dao_KuanDu[i]=r_border[i]-l_border[i];
        }
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     左边线V点检测
  @param     void
  @return    无
  Sample     Find_Left_Vpoint()
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Left_Vpoint=0;
uint8_t Left_V[2];
void Find_Left_Vpoint()
{
    Left_Vpoint=0;
    Left_V[0]=0;
    Left_V[1]=0;
    int i=0;
   for (i = 6; i < (data_stastics_l - 6); i++)
    {
        if((points_l[i][1] >= (Y_Border_Min + 1)) && (points_l[i][1] <= (Y_Border_Max - 1))&& points_l[i][1]>hightest+8)
        {
            if(points_l[i][1]>points_l[i+5][1] && points_l[i][1]>points_l[i+6][1] &&
                points_l[i][1]>points_l[i+7][1] &&
                points_l[i][1]>points_l[i-5][1] && points_l[i][1]>points_l[i-6][1] &&
                points_l[i][1]>points_l[i-7][1] && l_border[points_l[i][1]+3]<=2)
            {
                Left_Vpoint=1;
                Left_V[0]=points_l[i][0];
                Left_V[1]=points_l[i][1];
                break;
            }
        }
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     右边线V点检测
  @param     void
  @return    无
  Sample     Find_Right_Vpoint()
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Right_Vpoint=0;
uint8_t Right_V[2];
void Find_Right_Vpoint()
{
    // uint8_t Right_V2[2];
    Right_Vpoint=0;
    Right_V[0]=0;
    Right_V[1]=0;
    int i=0;
   for (i = 6; i < (data_stastics_r - 6); i++)
    {
        if((points_r[i][1] >= (Y_Border_Min + 1)) && (points_r[i][1] <= (Y_Border_Max - 1)) && points_r[i][1]>hightest+8)
        {
            if(points_r[i][1]>=points_r[i+5][1] && points_r[i][1]>points_r[i+6][1] &&
                points_r[i][1]>points_r[i+7][1] && 
                points_r[i][1]>=points_r[i-5][1] && points_r[i][1]>points_r[i-6][1] &&
                points_r[i][1]>points_r[i-7][1] && r_border[points_r[i][1]+5]>=152)
            {
                Right_Vpoint=1;
                Right_V[0]=points_r[i][0];
                Right_V[1]=points_r[i][1];
                break;
            }
        }
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     右边线A点检测，下角点
  @param     void
  @return    无
  Sample     Find_Right_Vpoint()
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Right_Apoint=0;
uint8_t Right_A[2];
void Find_Right_Apoint()
{
    Right_Apoint=0;
    Right_A[0]=0;
    Right_A[1]=0;
    int i=0;
    for(i = 6; i < (data_stastics_r - 6); i++)
    {
        if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 2)) && points_r[i][1]>hightest+8)
        {
            if(points_r[i][1]<points_r[i+5][1] && points_r[i][1]<points_r[i+6][1] &&
                points_r[i][1]<points_r[i+7][1] && 
                points_r[i][1]<points_r[i-5][1] && points_r[i][1]<points_r[i-6][1] &&
                points_r[i][1]<points_r[i-7][1] && r_border[points_r[i][1]-5]>=152 && r_border[points_r[i][1]]>40)
            {
                Right_Apoint=1;
                Right_A[0]=points_r[i][0];
                Right_A[1]=points_r[i][1];
                break;
            }
        }
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     左边线A点检测
  @param     void
  @return    无
  Sample     Find_Right_Vpoint()
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Left_Apoint=0;
uint8_t Left_A[2];
void Find_Left_Apoint()
{
    Left_Apoint=0;
    Left_A[0]=0;
    Left_A[1]=0;
    int i=0;
    for(i = 6; i < (data_stastics_l - 6); i++)
    {
        if((points_l[i][1] >= (Y_Border_Min + 2)) && (points_l[i][1] <= (Y_Border_Max - 2)) && points_l[i][1]>hightest+8)
        {
            if(points_l[i][1]<points_l[i+5][1] && points_l[i][1]<points_l[i+6][1] &&
                points_l[i][1]<points_l[i+7][1] && 
                points_l[i][1]<points_l[i-5][1] && points_l[i][1]<points_l[i-6][1] &&
                points_l[i][1]<points_l[i-7][1] && l_border[points_l[i][1]-5]<=2)
            {
                Left_Apoint=1;
                Left_A[0]=points_l[i][0];
                Left_A[1]=points_l[i][1];
                break;
            }
        }
    }
}
//寻找出环岛的点
/*-------------------------------------------------------------------------------------------------------------------
  @brief     右边线出环岛A点检测
  @param     void
  @return    无
  Sample     Find_AA_Right_Point()
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Right_AApoint=0;
uint8_t Right_AA[2];
void Find_AA_Right_Point()
{
    Right_AApoint=0;
    Right_AA[0]=0;
    Right_AA[1]=0;
    int i=0;
    for(i = 6; i < (data_stastics_l - 6); i++)
    {
        if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 2)) && points_r[i][1]>hightest+8)
        {
            if(points_r[i][0]<points_r[i+3][0] && points_r[i][0]<points_r[i+4][0] &&
                points_r[i][0]<points_r[i+5][0] && 
                points_r[i][0]<points_r[i-3][0] && points_r[i][0]<points_r[i-4][0] &&
                points_r[i][0]<points_r[i-5][0] )
            {
                Right_AApoint=1;
                Right_AA[0]=points_r[i][0];
                Right_AA[1]=points_r[i][1];
                break;
            }
        }
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     左边线出环岛A点检测
  @param     void
  @return    无
  Sample     Find_AA_Left_Point()
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Left_AApoint=0;
uint8_t Left_AA[2];
void Find_AA_Left_Point()
{
    Left_AApoint=0;
    Left_AA[0]=0;
    Left_AA[1]=0;
    int i=0;
    for(i = 6; i < (data_stastics_l - 6); i++)
    {
        if((points_l[i][1] >= (Y_Border_Min + 2)) && (points_l[i][1] <= (Y_Border_Max - 2)) && points_l[i][1]>hightest+8)
        {
            if(points_l[i][0]>points_l[i+3][0] && points_l[i][0]>points_l[i+4][0] &&
                points_l[i][0]>points_l[i+5][0] && 
                points_l[i][0]>points_l[i-3][0] && points_l[i][0]>points_l[i-4][0] &&
                points_l[i][0]>points_l[i-5][0] )
            {
                Left_AApoint=1;
                Left_AA[0]=points_l[i][0];
                Left_AA[1]=points_l[i][1];
                break;
            }
        }
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     特殊点检测函数
  @param     void
  @return    无
  Sample     Get_Special_Point()
  @note      寻找的拐点可以在内部修改
-------------------------------------------------------------------------------------------------------------------*/
//寻找特殊点
float Left_S=0;//赛道弯曲度
float Right_S=0;//赛道弯曲度
int LianXu_Left=0;//连续性检验
int LianXu_Right=0;
int Left_Up_Point=0;//一维边线拐点
int Left_Down_Point=0;
int Right_Up_Point=0;
int Right_Down_Point=0;
//测试函数
void Get_Special_Point()
{
    Find_Left_Apoint();
    Find_Right_Apoint();
    //出环岛A点检测
    Find_AA_Left_Point();
    Find_AA_Right_Point();
    //单边丢线
    Find_OneLeftLose_Point();
    Find_OneRightLose_Point();
    //入环岛V点
    Find_Left_Vpoint();
    Find_Right_Vpoint();
	//寻找拐点
    // Get_L_Up_Turn_Left_Point();
    // Get_L_Right_Turn_Up_Point();
    // Get_R_Up_Turn_Right_Point();
    // Get_R_Left_Turn_Up_Point();
	Get_L_Up_Turn_Left_Point_1(2);
	Get_L_Right_Turn_Up_Point_1(2);
	Get_R_Up_Turn_Right_Point_1(2);
	Get_R_Left_Turn_Up_Point_1(2);
    // //记录丢线
    Get_R_Border_Point_Num();
    Get_L_Border_Point_Num();
    Get_Relative_Border_Point_Num();
    // //计算赛道宽度
    // Get_SaiDao_Whith();
    // //连续性检验
    LianXu_Left=Continuity_Change_Left(start_point_l[1],20);//检测的终点在最高点的下5行，，防止误判（待测量）
    LianXu_Right=Continuity_Change_Right(start_point_r[1],20);
    //拐点检测
    Left_Up_Point=Find_Left_Up_Point((hightest+5),start_point_l[1],l_border);
    Left_Down_Point=Find_Left_Down_Point(start_point_l[1],(hightest+10),l_border);
    //下拐点判断条件不足，待测量
    Right_Up_Point=Find_Right_Up_Point((hightest+5),start_point_r[1],r_border);
    Right_Down_Point=Find_Right_Down_Point(start_point_r[1],(hightest+10),r_border);
    // //拐点检测
    // Left_Up_Point=Find_Left_Up_Point(hightest,start_point_l[1],Longest_Left_Line);
    // Left_Down_Point=Find_Left_Down_Point(start_point_l[1],(hightest+10),Longest_Left_Line);
    // //下拐点判断条件不足，待测量
    // Right_Up_Point=Find_Right_Up_Point(hightest,start_point_r[1],Longest_Right_Line);
    // Right_Down_Point=Find_Right_Down_Point(start_point_r[1],(hightest+10),Longest_Right_Line);
    // if(L_Up_Turn_Left_Point_Flag_1==1)
    // {
    //     Left_Monotonicity = Left_Monotonicity_Change(L_Up_Turn_Left_Point_1[1]-10,hightest,Longest_Left_Line);//检测左侧圆弧拐点
    // }
}
/********************************************工程函数*********************************************************/
/*-------------------------------------------------------------------------------------------------------------------
  @brief     计算赛道弯曲度函数
  @param     边线数组 边线长度
  @return    赛道弯曲度
  Sample     S_judge(Line ，N)
  @note      寻找的拐点可以在内部修改
-------------------------------------------------------------------------------------------------------------------*/
float Center_Jidge(uint8 *line)
{
    float S = 0.0f; // 初始化S
    int i=0;
    int Num_He=0;
    int num=0;
    for(i=start_point_l[1];i>hightest+1;i--)
    {
        Num_He+=line[i]-line[i-1];
        num++;
    }
    S=(float)Num_He/(float)num;
    return S;
}
//计算赛道弯曲度
float S_judge(uint16_t (*Line)[2], int N) 
{
    float S = 0.0f; // 初始化S
    float k, b;
    float x_avg = 0.0f, y_avg = 0.0f;
    int num_x = 0, num_y = 0; // 独立计数器
    int xy = 0, x_sq = 0;
    const int MAX_POINTS = (N < 240) ? N : 240; // 防止越界
    // 计算x_avg和num_x
    for (int i = 0; i < MAX_POINTS; i++) 
    {
        if (Line[i][1] == 254 || i == N) break;
        if (i < MAX_POINTS - 1 && Line[i][0] == Line[i + 1][0]) continue;
        x_avg += Line[i][1];
        num_x++;
    }
    if (num_x == 0) return -1.0f; // 避免除零
    x_avg /= num_x;
    // 计算y_avg和num_y
    for (int i = 0; i < MAX_POINTS; i++) 
    {
        if (Line[i][0] == 254 || i == N) break;
        if (i < MAX_POINTS - 1 && Line[i][0] == Line[i + 1][0]) continue;
        y_avg += Line[i][0];
        num_y++;
    }
    if (num_y == 0 || num_x != num_y) return -1.0f; // 确保数据一致性
    y_avg /= num_y;
    // 计算xy和x_sq
    for (int i = 0; i < MAX_POINTS; i++) 
    {
        if (Line[i][0] == 254 || Line[i][1] == 254 || i == N) break;
        if (i < MAX_POINTS - 1 && Line[i][0] == Line[i + 1][0]) continue;
        xy += Line[i][0] * Line[i][1];
        x_sq += Line[i][1] * Line[i][1];
    }
    // 计算斜率k和截距b
    float denominator = x_sq - num_x * x_avg * x_avg;
    if (denominator == 0) return -1.0f; // 避免除零
    k = (xy - num_x * x_avg * y_avg) / denominator;
    b = y_avg - k * x_avg;
    // 计算标准差S
    for (int i = 0; i < MAX_POINTS; i++) 
    {
        if (Line[i][0] == 254 || i == N) break;
        if (i < MAX_POINTS - 1 && Line[i][0] == Line[i + 1][0]) continue;
        float y_tg = k * Line[i][1] + b;
        S += (y_tg - Line[i][0]) * (y_tg - Line[i][0]);
    }
    if (num_x <= 1) return -1.0f; // 避免除零
    S = sqrt(S/(num_x - 1)); // 修正标准差公式
    return S;
}

// 辅助函数：交换两个整数
void swap(int *a, int *b) {
    int t = *a; *a = *b; *b = t;
}
// 辅助函数：限制值在 [min, max] 范围
int clamp(int val, int min, int max) {
    return (val < min) ? min : (val > max) ? max : val;
}

//连线函数输入两个坐标值，直接向连（x1，y1）在上（x2，y2）在下
void Link_Point(int x1, int x2, int y1, int y2, uint8 *Line) 
{
    if (y1 == y2) return; // 水平线无需处理
    // 统一方向：确保 y1 < y2
    int start_y = y1, end_y = y2;
    if (start_y > end_y) 
    {
        swap(&start_y, &end_y);
        swap(&x1, &x2); // 同步交换 x 坐标
    }
    // 越界保护
    start_y = clamp(start_y, 0, HEIGHT-1);
    end_y = clamp(end_y, 0, HEIGHT-1);
    // 浮点插值计算
    float dx = x2 - x1;
    float dy = end_y - start_y;
    for (int y = start_y; y <= end_y; y++) {
        float t = (float)(y - start_y) / dy;
        int hx = (int)(x1 + t * dx + 0.5f); // 四舍五入
        hx = clamp(hx, 0, WIDTH-1);
        Line[y] = (uint8)hx;
    }
}
void Add_Roundabout_Line(float k, int startX, int startY, int stopY, uint8 *Line)
{
    int i, t;
    // 限制startY范围
    startY = startY >= image_h-1 ? image_h-1 : (startY <= 0 ? 0 : startY);
    // 限制stopY范围
    stopY = stopY >= image_h-1 ? image_h-1 : (stopY <= 0 ? 0 : stopY);
    
    // 确保startY <= stopY
    if (startY > stopY)
    {
        t = startY;
        startY = stopY;
        stopY = t;
    }
    
    // 遍历y坐标，计算对应x值
    for (i = startY; i <= stopY; i++)
    {
        Line[i] = (int)((i - startY) / k + startX); // 正确公式
        // 限制x在有效范围内
        Line[i] = Line[i] >= image_w-3 ? image_w-3 : (Line[i] <= 2 ? 2 : Line[i]);
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     补线函数
  @param     起始点 终止点 边线数组
  @return    无
  Sample     Solpe_Add_Line(Start Stop,Line)
  @note      寻找的拐点可以在内部修改
-------------------------------------------------------------------------------------------------------------------*/
// 补线函数，从输入的点向上寻找5个点计算斜率，通过斜率补全下面的边线，实现补线
void Solpe_Add_Line(int Start, int Stop, uint8 *Line) {
    // 参数校验
    if (Start < 4 || Start >= HEIGHT || !Line) return; 
    // 计算斜率（Δy=5行）
    float k ;
    if(Start<=Stop)
    {
       k= (float)(Line[Start] - Line[Start-4]) / 5.0;//从上向下补线是计算Start上5个点的斜率
    }
    else if(Start>Stop)
    {
        k = (float)(Line[Start] - Line[Start+4]) / 5.0;//从下向上补线计算Start下的5个点的斜率
    }
    // 放宽阈值（根据场景调整）
    // if (fabs(k) > 10.0f) return;
    // 确定补线方向
    int step = (Stop >= Start) ? 1 : -1;
    int current = Start;
    int end = Stop + step; // 包含 Stop 行

    while (current != end) {
        current += step;
        if (current < 0 || current >= HEIGHT) break;
        // 计算插值
        int x = Line[Start] + (int)(k * (current - Start));
        x = clamp(x, 1, WIDTH-2);
        Line[current] = (uint8)x;
    }
}
/********************************************工程函数*********************************************************/
/*-------------------------------------------------------------------------------------------------------------------
  @brief     弯道判断函数
  @param     无
  @return    无
  Sample     Judge_WanDao()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
int WanDao_Flag=0;
void Judge_WanDao()//参数待定,,//或者检测单调性，最长白列位置
{
    WanDao_Flag=0;
    if(ZhiDao_Flag==0 && HuanDao_Stas==0)//直道和弯道意外进行判断
    {
        if(hightest>11 && (R_Border_Point_Num>15 || L_Border_Point_Num>15))//当最高点靠下，两边任意一边丢线时判断为弯道
        {
            WanDao_Flag=1;//弯道标志位进行辅助速度控制
        }
    }
}
//判断直道
/*-------------------------------------------------------------------------------------------------------------------
  @brief     左边线直道判断函数
  @param     无
  @return    无
  Sample     Left_ZhiDao_Judge()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
int ZhiDao_Flag=0;
int Left_ZhiDao_Flag=0;
int Right_ZhiDao_Flag=0;
float Left_Slope_A=0;
float Left_Slope_B=0;
float Left_Slope_All=0;
float Right_Slope_A=0;
float Right_Slope_B=0;
float Right_Slope_All=0;
void Left_ZhiDao_Judge()
{
    //数据清零
    // Left_Slope_A=0;
    // Left_Slope_B=0;
    // Left_Slope_All=0;
    // Left_ZhiDao_Flag=0;
    //计算斜率
    Left_Slope_A=Slope_Calculate(hightest,image_h/2,l_border);
    Left_Slope_B=Slope_Calculate(image_h/2,start_point_r[1],l_border);
    Left_Slope_All=Slope_Calculate(hightest,start_point_r[1],l_border);
    //判断是否为直线
    if(abs(Left_Slope_A-Left_Slope_B) <= 0.4 && 
        abs(Left_Slope_B-Left_Slope_All) <= 0.4 &&
        abs(Left_Slope_A-Left_Slope_All) <=0.4 )
    {
        Left_ZhiDao_Flag=1;
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     右边线直道判断函数
  @param     无
  @return    无
  Sample     Right_ZhiDao_Judge()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
void Right_ZhiDao_Judge()
{
    //数据清零
    // Right_Slope_A=0;
    // Right_Slope_B=0;
    // Right_Slope_All=0;
    // Right_ZhiDao_Flag=0;
    //计算斜率
    Right_Slope_A=Slope_Calculate(hightest,image_h/2,r_border);
    Right_Slope_B=Slope_Calculate(image_h/2,start_point_l[1],r_border);
    Right_Slope_All=Slope_Calculate(hightest,start_point_l[1],r_border);
    //判断是否为直线
    if(abs(Right_Slope_A-Right_Slope_B) <= 0.4 && 
        abs(Right_Slope_B-Right_Slope_All) <= 0.4 &&
        abs(Right_Slope_A-Right_Slope_All) <=0.4 )
    {
        Right_ZhiDao_Flag=1;
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     直道判断函数
  @param     判断直道 利用三段斜率进行直道判断 计算量较大
  @return    无
  Sample     Judge_ZhiDao1()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
void Judge_ZhiDao1()
{
    //数据清零
    Left_Slope_A=0;
    Left_Slope_B=0;
    Left_Slope_All=0;
    Right_Slope_A=0;
    Right_Slope_B=0;
    Right_Slope_All=0;
    //标志位清零
    Right_ZhiDao_Flag=0;
    Left_ZhiDao_Flag=0;
    if(Relative_Border_Point_Num<10)
    {
        Left_ZhiDao_Judge();
        Right_ZhiDao_Judge();
    }
    if(Right_ZhiDao_Flag==1 && Left_ZhiDao_Flag==1)
    {
        ZhiDao_Flag=1;
    }
    else
    {
        ZhiDao_Flag=0;
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     直道判断函数
  @param     简单直道判断
  @return    无
  Sample     Judge_ZhiDao2()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
void Judge_ZhiDao2()
{
    ZhiDao_Flag=0;
    if(Relative_Border_Point_Num<=10)
    {
        if(LianXu_Left<=10 && LianXu_Right<=10 && hightest<5)
        {
            if(Mid_Error<10 && Mid_Error>-10)
            {
                ZhiDao_Flag=1;
            }
        }
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     十字判断函数
  @param     寻找四个拐点
  @return    无
  Sample     Judge_ShiZi()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
int ShiZi_Flag2=0;//十字标志位
void Judge_ShiZi(void)
{
    if(HuanDao_Stas==0 && Relative_Border_Point_Num>10)
    {
        //检测到生长方向的四个角点，十字状态1，直接连接四个角点,拐点位置疑似不符合十字条件，或者拐点位置便宜
        // if(Left_Up_Point!=0 && Right_Up_Point!=0 && abs(Left_Up_Point-Right_Up_Point)<10 && 
        //     Left_Down_Point!=0 && Right_Down_Point!=0 && abs(Left_Down_Point-Right_Down_Point)<10 && 
        //     Left_Up_Point<Left_Down_Point && Right_Up_Point<Right_Down_Point)
        // {
        //     ShiZi_Flag2=1;
        //     Link_Point(l_border[Left_Up_Point],l_border[Left_Down_Point],Left_Up_Point,Left_Down_Point,l_border);
        //     Link_Point(r_border[Right_Up_Point],r_border[Right_Down_Point],Right_Up_Point,Right_Down_Point,r_border);
        // }
        // else if(Left_Up_Point!=0 && Right_Up_Point!=0 && abs(Left_Up_Point-Right_Up_Point)<10 && 
        //     Left_Down_Point!=0 && Left_Up_Point<Left_Down_Point)
        // {
        //     ShiZi_Flag2=2;
        //     Link_Point(l_border[Left_Up_Point],l_border[Left_Down_Point],Left_Up_Point,Left_Down_Point,l_border);
        //     Link_Point(r_border[Right_Up_Point],X_Border_Max,Right_Up_Point,Y_Border_Max,r_border);
        // }
        // else if(Left_Up_Point!=0 && Right_Up_Point!=0 && abs(Left_Up_Point-Right_Up_Point)<10 && 
        //         Right_Down_Point!=0 && Right_Up_Point<Right_Down_Point)
        // {
        //     ShiZi_Flag2=3;
        //     Link_Point(r_border[Right_Up_Point],r_border[Right_Down_Point],Right_Up_Point,Right_Down_Point,r_border);
        //     Link_Point(l_border[Left_Up_Point],X_Border_Min,Left_Up_Point,Y_Border_Max,l_border);
        // }
        // else if(Left_Up_Point!=0 && Right_Up_Point!=0 && abs(Left_Up_Point-Right_Up_Point)<10)
        // {
        //     ShiZi_Flag2=4;
        //     Link_Point(l_border[Left_Up_Point],X_Border_Min,Left_Up_Point,Y_Border_Max,l_border);
        //     Link_Point(r_border[Right_Up_Point],X_Border_Max,Right_Up_Point,Y_Border_Max,r_border);
        // }
        if( L_Right_Turn_Up_Point_Flag_1==1 && R_Left_Turn_Up_Point_Flag_1==1 && //出现四个拐点
            R_Up_Turn_Right_Point_Flag_1==1 && L_Up_Turn_Left_Point_Flag_1==1 && 
            abs(L_Right_Turn_Up_Point_Position_1-R_Left_Turn_Up_Point_Position_1)<5 && //并且左右拐点位置相差不大
            abs(L_Up_Turn_Left_Point_Position_1-R_Up_Turn_Right_Point_Position_1)<10 &&      
            (L_Right_Turn_Up_Point_Position_1 > L_Up_Turn_Left_Point_Position_1) && //上拐点位置在上，下拐点位置在下，，摄像头视角太远了
            (R_Left_Turn_Up_Point_Position_1 > R_Up_Turn_Right_Point_Position_1) && 
            R_Left_Turn_Up_Point_1[0]-L_Right_Turn_Up_Point_1[0]>20)//出显了很多干扰
        {
                ShiZi_Flag2=1;
                //四个点直接相连
                Link_Point(L_Right_Turn_Up_Point_1[0],L_Up_Turn_Left_Point_1[0],L_Right_Turn_Up_Point_1[1],L_Up_Turn_Left_Point_1[1],l_border);
                Link_Point(R_Left_Turn_Up_Point_1[0],R_Up_Turn_Right_Point_1[0],R_Left_Turn_Up_Point_1[1],R_Up_Turn_Right_Point_1[1],r_border);
        }//
        else if( Left_Up_Point!=0 && Right_Up_Point!=0 && //由于生长方向的上拐点位置偏移严重选择，一位边线的撕裂点作为拐点进行沿斜率补线，
                R_Up_Turn_Right_Point_Flag_1==1 && //并未处理一维边线上拐点，，是否正确（目前没问题）
                R_Left_Turn_Up_Point_Flag_1==1 &&
                abs(Left_Up_Point-Right_Up_Point)<=5 && //并且左右拐点位置相差不大
                R_Up_Turn_Right_Point_Position_1 < 30  && //下拐点的生长方向锁的太偏下了可能导致,状态2不能识别，，，在距离十字较远处无法识别到十字
                (R_Left_Turn_Up_Point_Position_1-R_Up_Turn_Right_Point_Position_1)>=10 &&
                R_Left_Turn_Up_Point_1[0]-L_Right_Turn_Up_Point_1[0]>20)
        {
                ShiZi_Flag2=2;
                //确定两点进行连线
                Link_Point(R_Left_Turn_Up_Point_1[0],R_Up_Turn_Right_Point_1[0],R_Left_Turn_Up_Point_1[1],R_Up_Turn_Right_Point_1[1],r_border);
                //十字补线直接将线拉倒最下角
                // Link_Point(Left_Up_Point,Y_Border_Max,l_border[Left_Up_Point],X_Border_Min,r_border);
                Solpe_Add_Line(Left_Up_Point,79,l_border);
        }
        else if(Left_Up_Point!=0 && Right_Up_Point!=0 && //与状态2相同
                L_Right_Turn_Up_Point_Flag_1==1 && 
                L_Up_Turn_Left_Point_Flag_1==1 && 
                abs(Left_Up_Point-Right_Up_Point)<=5 && 
                L_Up_Turn_Left_Point_Position_1<30 && //与状态2同理
                (L_Right_Turn_Up_Point_Position_1-L_Up_Turn_Left_Point_Position_1)>=10 && 
                R_Left_Turn_Up_Point_1[0]-L_Right_Turn_Up_Point_1[0]>20)
        {
                ShiZi_Flag2=3;
                //确定两点进行连线
                Link_Point(L_Right_Turn_Up_Point_1[0],L_Up_Turn_Left_Point_1[0],L_Right_Turn_Up_Point_1[1],L_Up_Turn_Left_Point_1[1],l_border);
                //十字补线直接将线拉倒最下角
                // Link_Point(Right_Up_Point,Y_Border_Max,r_border[Right_Up_Point],X_Border_Max,l_border);
                Solpe_Add_Line(Right_Up_Point,79,r_border);
        }
        else if(Left_Up_Point!=0 && Right_Up_Point!=0 && 
                Left_Up_Point>=40 && Right_Up_Point>=40 &&
                abs(Left_Up_Point-Right_Up_Point)<=5 && 
                r_border[Right_Up_Point]-l_border[Left_Up_Point]>20)//只有上边两个点，，待进行误判检测，，，之前的代码很可能会产生误判在弯道等大面丢线的情况下误判
        {
                ShiZi_Flag2=4;
                //十字补线直接将线拉倒最下角
                // Link_Point(Right_Up_Point,Y_Border_Max,r_border[Right_Up_Point],X_Border_Max,l_border);
                // Link_Point(Left_Up_Point,Y_Border_Max,l_border[Left_Up_Point],X_Border_Min,r_border);
                //  将左侧边线以一定斜率补线
                Solpe_Add_Line(Left_Up_Point,79,l_border);
                // //将右侧边线以一定斜率补线
                Solpe_Add_Line(Right_Up_Point,79,r_border);
        }
        else//未识别到任何拐点，，概率不大，，进入十字判断后其他的点很可能会干扰判断
        {
            ShiZi_Flag2=0;
        }
    }
    else//未满足十字判断的前至条件
    {
        ShiZi_Flag2=0;
    }
}
//环岛判断
int HuanDao_Flag=0;//环岛标志位
int HuanDao_Stas=0;//环岛状态机
int Left_Monotonicity=0;//左环岛圆弧拐点Y坐标
int Right_Monotonicity=0;//右环岛圆弧拐点Y坐标
int A_Point=0;//记录最后一个A点的Y坐标
int A_Point2=0;
//二维数组判断更加方便，，只需将一位数组和二维数组对应即可
//环岛进入环岛时机早，，切内弯进入环岛是能有效避免误判
/*-------------------------------------------------------------------------------------------------------------------
  @brief     环岛退出函数
  @param     换到标志位计时，50幅图像环岛状态未改变将退出环岛
  @return    无
  Sample     HuanDao_Break()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
int Image_HuanDao=0;
int Compare_HuaDao_Image=50;
int Last_HuanDao_Stace=0;
void HuanDao_Break(void)
{
    //如果环岛状态没有改变，Image_HuanDao开始累加
    if(HuanDao_Stas!=0 && HuanDao_Stas==Last_HuanDao_Stace)
    {
        Image_HuanDao++;
    }
    else if(HuanDao_Stas!=Last_HuanDao_Stace || HuanDao_Stas==0 )//当退出环岛状态或环岛状态改变时，数据清零
    {
        Image_HuanDao=0;
    }
    //当在圆环内部时，累加时间变长
    if((HuanDao_Stas==5 || HuanDao_Stas==12) && Speed_Target<=450)
    {
        Compare_HuaDao_Image=150;
    }
    else if(HuanDao_Stas==5 || HuanDao_Stas==12)
    {
        Compare_HuaDao_Image=100;
    }
    else
    {
        Compare_HuaDao_Image=50;
    }
     if(HuanDao_Stas==7 || HuanDao_Stas==14)
    {
        if(Speed_Target>=450)
        {
            Compare_HuaDao_Image=4;
        }
         Compare_HuaDao_Image=10;
    }
    //达到累加时间 退出环岛
    if(Image_HuanDao>Compare_HuaDao_Image)
    {
        Image_HuanDao=0;
        HuanDao_Stas=0;
    }
    if(HuanDao_Stas==7 && WanDao_Flag==1)
    {
         HuanDao_Stas=0;
    }
    if(HuanDao_Stas==14 && WanDao_Flag==1)
    {
         HuanDao_Stas=0;
    }
    Last_HuanDao_Stace=HuanDao_Stas;
} 
//右环岛第一个不在边界点上的点，可以作为辅助判断条件
/*-------------------------------------------------------------------------------------------------------------------
  @brief     右环岛第一个不在边界点上的点
  @param     右环岛第一个不在边界点上的点，可以作为辅助判断条件
  @return    无
  Sample     First_Point_Huandao_Right()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
int R_First_Point_Huandao=0;
void First_Point_Huandao_Right()
{
    int i=0;
    R_First_Point_Huandao=0;
    for(i=start_point_r[1];i<30;i--)
    {   
        if(r_border[i]!=152)
        {
            R_First_Point_Huandao=i;
        }
    }
}
//左环岛第一个不在边界上的点，可以作为辅助判断条件
/*-------------------------------------------------------------------------------------------------------------------
  @brief     左环岛第一个不在边界点上的点
  @param     左环岛第一个不在边界点上的点，可以作为辅助判断条件
  @return    无
  Sample     First_Point_Huandao_Left()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
int L_First_Point_Huandao1=0;
void First_Point_Huandao_Left()
{
    int i=0;
    L_First_Point_Huandao1=0;
    for(i=start_point_l[1];i<30;i--)
    {   
        if(l_border[i]!=2)
        {
            L_First_Point_Huandao1=i;
        }
    }
}
//寻找低角入环时机的环岛处理
void Judge_HuanDao2()
{
    int i=0;
    Right_Center=0;
    Left_Center=0;
    if(HuanDao_Stas==0 && ShiZi_Flag2==0)//环岛判断的前置条件，，不满足这写条件是不会进行环岛判断的,,增加判断条件（左右丢线数不一样）
    {
        //两个圆弧拐点并不一定要满足（可以去掉）（未经过测量）
        if(L_Up_Turn_Left_Point_Flag_1==1 && R_Up_Turn_Right_Point_Flag_1==0 && L_Border_Point_Num>15 && R_Border_Point_Num<10)//左右下拐点单独出现，，可以进行圆弧拐点识别
        {
            //一维数组进行圆弧判断
            Left_Monotonicity = Left_Monotonicity_Change(L_Up_Turn_Left_Point_1[1]-10,hightest+10,Longest_Left_Line);//检测左侧圆弧拐点
            if(HuanDao_Stas==0 && LianXu_Right==0 && LianXu_Left!=0 && //左不连续，，右连续
                L_Up_Turn_Left_Point_Flag_1==1 &&  Left_Monotonicity!=0 && (Left_Monotonicity-L_Up_Turn_Left_Point_1[1]<-10))//判断为左环岛
            {
                HuanDao_Flag=1;//左环岛
                HuanDao_Stas=1;//进入左环岛状态机
            }else 
            {
                HuanDao_Flag=0;//环岛标志位清零
            }
        }
        else if(R_Up_Turn_Right_Point_Flag_1==1 && L_Up_Turn_Left_Point_Flag_1==0 && R_Border_Point_Num>15 && L_Border_Point_Num<10)//左右下拐点单独出现，
        {
            Right_Monotonicity=Right_Monotonicity_Change(R_Up_Turn_Right_Point_1[1]-10,hightest+10,Longest_Right_Line);//检测右侧圆弧拐点
            if(HuanDao_Stas==0 && LianXu_Right!=0 && LianXu_Left==0 &&//左连续，，右不连续
                R_Up_Turn_Right_Point_Flag_1==1 &&  Right_Monotonicity!=0 && (Right_Monotonicity-R_Up_Turn_Right_Point_1[1]<-10)//判断为右环岛
            )
            {
                HuanDao_Flag=2;//右环岛
                HuanDao_Stas=8;//进入右环岛状态机
            }
            else 
            {
                HuanDao_Flag=0;//环岛标志位清零
            }
        }
        else
        {
             HuanDao_Flag=0;//环岛标志位清零
        }
    }
    if(HuanDao_Stas==1)//初见环岛，，将圆弧拐点与下拐点连线连线，
    {
        //圆弧拐点误判为上转右拐点，，（难办）
        // Left_Monotonicity = Left_Monotonicity_Change(L_Up_Turn_Left_Point_1[1]-10,hightest,Longest_Left_Line);//检测左侧圆弧拐点
        // if(Left_Monotonicity!=0 &&   L_Up_Turn_Left_Point_Flag_1==1 &&  L_Up_Turn_Left_Point_Position_1<100 && //出现圆弧拐点
        //     (L_Up_Turn_Left_Point_1[1]>Left_Monotonicity))//下拐点和圆弧拐点符合环岛位置，，进行连线
        // {
        //     A_Point2=L_Up_Turn_Left_Point_1[1];//用来检测左下拐点消失，，记录的时满足环岛状态1的下拐点
        //     Right_Center=1;//单边巡线
        //     // Link_Point(l_border[Left_Monotonicity],L_Up_Turn_Left_Point_1[0],Left_Monotonicity,L_Up_Turn_Left_Point_1[1],l_border);//连线函数
        // } //环岛状态机进行跳变的条件，，当左下角拐点消失（同时一维数组的圆弧拐点同时消失）此圆弧拐点任务结束//检验上转左拐点位置，尝试修改一下判断条件
        int White_Huandao=0;
        int Baidain_Huandao=0;
        Right_Center=1;
        for(i=start_point_r[1];i>60;i--)
        {
            if(My_Image[i][3]==255)
            {
                White_Huandao++;
            }
            if(White_Huandao>5)
            {
                Baidain_Huandao=1;
                break;
            }
        }
        if(HuanDao_Stas==1 && LianXu_Right==0 && Baidain_Huandao==1 && Left_Apoint==0)
        {
            //当下拐点跳跃时说明左下拐点已经消失
            HuanDao_Stas=2;
            Right_Center=1;
        }
    }
    else if(HuanDao_Stas==2)//初入环岛，，左下角拐点消失，将圆弧拐点与图像左下角连接
    {
        // //限制圆弧识别范围，，减少误判
        // if(L_Right_Turn_Up_Point_Flag_1==1 && R_Left_Turn_Up_Point_Position_1>100)
        // {
        //     Left_Monotonicity = Left_Monotonicity_Change(85,L_Right_Turn_Up_Point_1[1]+6,l_border);//检测左侧圆弧拐点
        // }
        // else
        // {
        //     Left_Monotonicity = Left_Monotonicity_Change(85,hightest+10,l_border);//检测左侧圆弧拐点
        // }
        // //对于下拐点的判断生长方向判断更加准确，，一维数组的撕裂点的判断条件右待优化
        // if( Left_Monotonicity!=0)//识别到圆弧拐点
        // {
        //     Link_Point(l_border[Left_Monotonicity],X_Border_Min,Left_Monotonicity,Y_Border_Max,l_border);//连线函数
        // }
        //检测左下角黑点，状态一和状态三有重叠部分
        // int Black_Huandao=0;
        // int Heidain_Huandao=0;
        // Right_Center=1;
        // for(i=start_point_l[1];i>60;i--)
        // {
        //     if(My_Image[i][3]==0)
        //     {
        //         Black_Huandao++;
        //     }
        //     if(Black_Huandao>5)
        //     {
        //         Heidain_Huandao=1;
        //         break;
        //     }
        // }
        if(HuanDao_Stas==2 && Left_Vpoint==1 && Left_Up_Point>20)//当二维数组圆弧拐点消失，，提前进入下一个状态提前打角，防止撤回一个环岛
        {
            Right_Center=0;
            HuanDao_Stas=3;//进入状态3
        }
    }
    else if(HuanDao_Stas==3)//连接V点进行补线，，，进入环岛,,将右边线封死
    {
        //一维数组上拐点位置可能不在V点上（尤其距离V点越近偏移越严重）
        //可以尝试使用上拐点（生长方向进行连线但是生长方向的点智能作为辅助，因为生长方向经常识别不到V点，导致无法进入环岛）
        //无法完全避免V点偏移除非增减识别V点的函数（Y坐标大与周围点的点视为V点）只有在近距离才能检测到
        //进环岛的右边线可以增大斜率，，保证速度快的时候也可以进入环岛
        // Left_Monotonicity = Left_Monotonicity_Change(115,hightest);//检测左侧圆弧拐点
        if(L_Right_Turn_Up_Point_Flag_1==1)//使用两种拐点进行连线，，增加稳定性
        {
            Link_Point(L_Right_Turn_Up_Point_1[0],X_Border_Max,L_Right_Turn_Up_Point_1[1],(Y_Border_Max),r_border);//连线函数
            Link_Point(L_Right_Turn_Up_Point_1[0],(X_Border_Min),L_Right_Turn_Up_Point_1[1],hightest,r_border);//连线函数
        }
        else if(Left_Vpoint!=0)
        {
            Link_Point(Left_V[0],X_Border_Max,Left_V[1],Y_Border_Max,r_border);//连线函数
            Link_Point(Left_V[0],(X_Border_Min),Left_V[1],hightest,r_border);//连线函数
        } 
        // else if(L_Right_Turn_Up_Point_Flag_1==1)//使用两种拐点进行连线，，增加稳定性
        // {
        //     Link_Point(L_Right_Turn_Up_Point_1[0],X_Border_Max,L_Right_Turn_Up_Point_1[1],(Y_Border_Max),r_border);//连线函数
        //     Link_Point(L_Right_Turn_Up_Point_1[0],(X_Border_Min),L_Right_Turn_Up_Point_1[1],hightest,r_border);//连线函数
        // }
        // else if(Left_Up_Point!=0)
        // {
        //     Link_Point(l_border[Left_Up_Point],X_Border_Max,Left_Up_Point,Y_Border_Max,r_border);//连线函数
        //     Link_Point(l_border[Left_Up_Point],(X_Border_Min),Left_Up_Point,hightest,r_border);//连线函数
        // }
        //一维数组上拐点消失，，或这生长方向的拐点消失，，组合判断V点
        if(HuanDao_Stas==3 && Left_Up_Point==0 && Left_Vpoint==0 && L_Right_Turn_Up_Point_Flag_1==0)
        {
            HuanDao_Stas=4;//进入状态4
        }
    }
    else if(HuanDao_Stas==4)//V点从左下边线变到右边线
    {
        if(R_Left_Turn_Up_Point_Flag_1==1)
        {
            Link_Point(R_Left_Turn_Up_Point_1[0],X_Border_Max,R_Left_Turn_Up_Point_1[1],(Y_Border_Max),r_border);//连线函数
            Link_Point(R_Left_Turn_Up_Point_1[0],(X_Border_Min),R_Left_Turn_Up_Point_1[1],(hightest),r_border);//连线函数
        }
        else if(Right_Vpoint!=0)
        {
            Link_Point(Right_V[0],X_Border_Max,Right_V[1],(Y_Border_Max),r_border);//连线函数
            Link_Point(Right_V[0],(X_Border_Min),Right_V[1],(hightest),r_border);//连线函数
        }
        // else if(Right_Up_Point!=0)
        // {
        //     Link_Point(r_border[Right_Up_Point],X_Border_Max,Right_Up_Point,(Y_Border_Max),r_border);//连线函数
        //     Link_Point(r_border[Right_Up_Point],(X_Border_Min),Right_Up_Point,(hightest),r_border);//连线函数
        // }
        //组合判断V点
        if(HuanDao_Stas==4 && R_Left_Turn_Up_Point_Flag_1==0 && Right_Vpoint==0)
        {
            HuanDao_Stas=5;//进入状态5
        }
    }
    else if(HuanDao_Stas==5)//成功进入环岛
    {
        //单边巡线
        //正常巡线，，寻找出环岛的点
        if(HuanDao_Stas==5 && Right_AApoint==1)
        {
            HuanDao_Stas=6;//进入状态6
        }
    }
    else if(HuanDao_Stas==6)//出环岛
    {
        //出环岛也将右边线封死，，至到出现左上拐点或则右边线连续
        if(Right_AApoint==1)
        {
            //记录最后一个拐点
            A_Point=Right_AA[1];
            Link_Point(Right_AA[0],(X_Border_Min),Right_AA[1],hightest,r_border);//连线函数
        }
        else if(Right_AApoint==0)
        {
            //用记录的拐点补线
            Link_Point((r_border[A_Point]),(X_Border_Min),(A_Point),(hightest),r_border);//连线函数
        }
        // Add_Roundabout_Line(0.9,X_Border_Min,hightest,Y_Border_Max,r_border);
        // Link_Point(X_Border_Min,X_Border_Max,hightest,Y_Border_Max,r_border);
        if(HuanDao_Stas==6 && LianXu_Right==0 && Left_Vpoint!=0 && LianXu_Left!=0)//寻找到左上拐点时进入状态7或者当右边线连续的时后跳转到下一个状态
        {
            HuanDao_Stas=7;
        }
    }
    else if(HuanDao_Stas==7)//使出环岛
    {
        // if(Left_Up_Point!=0)//将上拐点向下连线
        // {
        //    Link_Point(l_border[Left_Up_Point],X_Border_Min,Left_Up_Point,Y_Border_Max,l_border);
        // }
        // else if (Left_Vpoint==1)
        // {
        //     Link_Point(Left_V[0],X_Border_Min,Left_V[1],Y_Border_Max,l_border);
        // }
        Right_Center=1;
        if(HuanDao_Stas==7 && Left_Up_Point==0 && Left_Vpoint==0 && LianXu_Right==0)
        {
            Right_Center=0;
            HuanDao_Stas=0;//左环岛结束
            HuanDao_Flag=0;
        }
    }
    //右环岛状态机
    if(HuanDao_Stas==8)//初见环岛
    {
        // Right_Monotonicity=Right_Monotonicity_Change(R_Up_Turn_Right_Point_1[1]-10,hightest,Longest_Right_Line);//检测右侧圆弧拐点
        // if(Right_Monotonicity!=0 && R_Up_Turn_Right_Point_Flag_1==1 && R_Up_Turn_Right_Point_Position_1<100 &&//右边圆弧出现，，下拐点出现
        //     (Right_Monotonicity<R_Up_Turn_Right_Point_1[1]))//下拐点出现在圆弧下边
        // {
        //     A_Point2=R_Up_Turn_Right_Point_1[1];//用来检测左下拐点消失，，记录的时满足环岛状态8的下拐点
        //     Left_Center=1;
        //     // Link_Point(r_border[Right_Monotonicity],R_Up_Turn_Right_Point_1[0],Right_Monotonicity,R_Up_Turn_Right_Point_1[1],r_border);//连线函数
        // }
        // int R_Huandao_A_Point=0;
        // Right_Monotonicity=Right_Monotonicity_Change(R_Up_Turn_Right_Point_1[1]-10,hightest,Longest_Right_Line);//检测右侧圆弧拐点
        // if(Right_Monotonicity!=0 && R_Up_Turn_Right_Point_Flag_1==1)
        // {
        //     if(r_border[R_Up_Turn_Right_Point_1[1]+5]==157 && r_border[R_Up_Turn_Right_Point_1[1]-1]!=157)
        //     {
        //         R_Huandao_A_Point=1;
        //     }
        // }
        int White_Huandao=0;
        int Baidain_Huandao=0;
        Left_Center=1;
        for(i=start_point_r[1];i>60;i--)
        {
            if(My_Image[i][152]==255)
            {
                White_Huandao++;
            }
            if(White_Huandao>5)
            {
                Baidain_Huandao=1;
                break;
            }
        }
        if(HuanDao_Stas==8 && LianXu_Left==0 && Baidain_Huandao==1 && Right_Apoint==0)//下拐点突变，，左边连续
        {
            //当下拐点跳跃时说明左下拐点已经消失
            HuanDao_Stas=9;
            Left_Center=1;
        }
    }
    else if(HuanDao_Stas==9)//初入环岛
    {
        // //圆弧拐点的位置选取
        // if(R_Left_Turn_Up_Point_Flag_1==1 && R_Left_Turn_Up_Point_Position_1>100)
        // {
        //     Right_Monotonicity = Right_Monotonicity_Change(85,R_Left_Turn_Up_Point_1[1]+8,r_border);//检测右侧圆弧拐点
        // }
        // else
        // {
        //     Right_Monotonicity = Right_Monotonicity_Change(85,hightest+10,r_border);//检测右侧圆弧拐点
        // }
        // if( Right_Monotonicity!=0)//识别到圆弧拐点
        // {
        //     Link_Point(r_border[Right_Monotonicity],X_Border_Max,Right_Monotonicity,Y_Border_Max,r_border);//连线函数
        // }
        // for(i = 7; i < (data_stastics_r - 5); i++)
        // {
        //     if((points_r[i][1] >= (Y_Border_Min + 2)) && (points_r[i][1] <= (Y_Border_Max - 2)))
        //     {
        //         if((points_r[i][0] < points_r[i - 7][0]) && (points_r[i][0] < points_r[i - 4][0]) && (points_r[i][0] <= points_r[i - 1][0]) &&
        //            (points_r[i][0] <= points_r[i + 1][0]) && (points_r[i][0] < points_r[i + 3][0]) && (points_r[i][0] < points_r[i + 5][0]) &&
        //            (dir_r[i + 1] == 3 || dir_r[i + 1] == 4) && (dir_r[i + 3] == 3 || dir_r[i + 3] == 4) &&
        //            (dir_r[i - 1] == 4 || dir_r[i + 1] == 5) && (dir_r[i - 3] == 4 || dir_r[i - 3] == 5) && 
        //             r_border[i-7]!=157 &&  r_border[i+5]!=157)                    
        //         {
        //             Right_Monotonicity=i;
        //         }
        //     }
        // }
        // int Black_Huandao=0;
        // int Heidain_Huandao=0;
        // Left_Center=1;
        // for(i=start_point_r[1];i>60;i--)
        // {
        //     if(My_Image[i][157]==0)
        //     {
        //         Black_Huandao++;
        //     }
        //     if(Black_Huandao>4)
        //     {
        //         Heidain_Huandao=1;
        //         break;
        //     }
        // }
        if(HuanDao_Stas==9 && Right_Vpoint==1  && LianXu_Left==0 )//当二维数组圆弧拐点消失，上拐点出现在某一位置时，提前进入下一个状态提前打角，防止撤回一个环岛
        {
            Left_Center=0;
            HuanDao_Stas=10;//进入状态10
        }
    }
    else if(HuanDao_Stas==10)//右边线出现V点。。链接V点到左下位置
    {
        //进入环岛的链接点可以链接到扫线最高的那一行hightest
        //一维数组上拐点位置可能不在V点上（尤其距离V点越近偏移越严重）
        //可以尝试使用上拐点（生长方向进行连线但是生长方向的点智能作为辅助，因为生长方向经常识别不到V点，导致无法进入环岛）
        //无法完全避免V点偏移除非增减识别V点的函数（Y坐标大与周围点的点视为V点）只有在近距离才能检测到
        //进环岛的右边线可以增大斜率，，保证速度快的时候也可以进入环岛
        // Right_Monotonicity = Right_Monotonicity_Change(115,hightest);//检测右侧圆弧拐点
        if(R_Left_Turn_Up_Point_Flag_1==1)//使用两种拐点进行连线，，增加稳定性
        {
            Link_Point(R_Left_Turn_Up_Point_1[0],X_Border_Min,R_Left_Turn_Up_Point_1[1],(Y_Border_Max),l_border);//连线函数，向下连
            Link_Point(R_Left_Turn_Up_Point_1[0],(X_Border_Max),R_Left_Turn_Up_Point_1[1],hightest,l_border);//连线函数，向上连
        }
        else if(Right_Vpoint!=0)//当左转上拐点不稳定时补充右上拐点连线保证进入环岛
        {
            Link_Point(Right_V[0],X_Border_Min,Right_V[1],Y_Border_Max,l_border);//连线函数
            Link_Point(Right_V[0],(X_Border_Max),Right_V[1],hightest,l_border);//连线函数
        }
        // else if(R_Left_Turn_Up_Point_Flag_1==1)//使用两种拐点进行连线，，增加稳定性
        // {
        //     Link_Point(R_Left_Turn_Up_Point_1[0],X_Border_Min,R_Left_Turn_Up_Point_1[1],(Y_Border_Max),l_border);//连线函数，向下连
        //     Link_Point(R_Left_Turn_Up_Point_1[0],(X_Border_Max),R_Left_Turn_Up_Point_1[1],hightest,l_border);//连线函数，向上连
        // }
        // else if(Right_Up_Point!=0)//当左转上拐点不稳定时补充右上拐点连线保证进入环岛
        // {
        //     Link_Point(r_border[Right_Up_Point],X_Border_Min,Right_Up_Point,Y_Border_Max,l_border);//连线函数
        //     Link_Point(r_border[Right_Up_Point],(X_Border_Max),Right_Up_Point,hightest,l_border);//连线函数
        // }
        //一维数组上拐点消失，，或这生长方向的拐点消失
        if(HuanDao_Stas==10 && Right_Vpoint==0 && R_Left_Turn_Up_Point_Flag_1==0)
        {
            HuanDao_Stas=11;//进入状态4
        }
    }
    else if(HuanDao_Stas==11)//V点跑到左边线，，这个状态对环岛的影响比较小
    {
        if(L_Right_Turn_Up_Point_Flag_1==1)//生长方向识别到的V点，，连接左下角
        {
            Link_Point(L_Right_Turn_Up_Point_1[0],X_Border_Min,L_Right_Turn_Up_Point_1[1],(Y_Border_Max),l_border);//连线函数
            Link_Point(L_Right_Turn_Up_Point_1[0],(X_Border_Max),L_Right_Turn_Up_Point_1[1],(hightest),l_border);//连线函数
        }
        else if(Left_Vpoint==1)
        {
            Link_Point(Left_V[0],X_Border_Min,Left_V[1],Y_Border_Max,l_border);//连线函数
            Link_Point(Left_V[0],(X_Border_Max),Left_V[1],hightest,l_border);//连线函数
        }
        // else if(Left_Up_Point!=0)//当生长方向识别不稳定时，，补充左拐点进行补线，，左拐点位置不准确但一定会出现，，可以保证进入环岛
        // {
        //     Link_Point(l_border[Left_Up_Point],X_Border_Min,Left_Up_Point,(Y_Border_Max),l_border);//连线函数
        //     Link_Point(l_border[Left_Up_Point],(X_Border_Max),Left_Up_Point,(hightest),l_border);//连线函数，，连接到hightest确保驶入环岛
        // }
        if(HuanDao_Stas==11 && Left_Vpoint==0 && L_Right_Turn_Up_Point_Flag_1==0)//V点从左边线消失
        {
            HuanDao_Stas=12;//进入状态12
        }
    }
    else if(HuanDao_Stas==12)//进入环岛中，正常巡线
    {
        //正常巡线，，寻找出环岛的点
        if(HuanDao_Stas==12 && Left_AApoint==1)
        {
            HuanDao_Stas=13;//进入状态13
        }
    }
    else if(HuanDao_Stas==13)//出环岛
    {
        //出环岛也将左边线封死，，至到出现右上拐点或则左边线连续
        if(Left_AApoint==1)
        {
            //记录最后一个拐点
            A_Point=Left_AA[1];
            Link_Point(Left_AA[0],X_Border_Max,Left_AA[1],hightest,l_border);//连线函数，，连到搜素最高的y值
        }
        else if(R_Up_Turn_Right_Point_Flag_1==0)
        {
            //用记录的拐点补线
            Link_Point((l_border[A_Point]),X_Border_Max,(A_Point),hightest,l_border);//连线函数
        }
        if(HuanDao_Stas==13 && LianXu_Left==0 && LianXu_Right!=0 && Right_Vpoint!=0)//寻找到左上拐点时进入状态7或者当右边线连续的时后跳转到下一个状态
        {
            HuanDao_Stas=14;
        }
    }
    else if (HuanDao_Stas==14)//识别右上拐点，，连线，直道使出环岛
    {
        // if(Right_Up_Point!=0)//将上拐点向下连线
        // {
        //    Link_Point(r_border[Right_Up_Point],X_Border_Max,Right_Up_Point,Y_Border_Max,r_border);//线下连钱，成功使出环岛
        // }
        // else if(Right_Vpoint==1)//将上拐点向下连线
        // {
        //    Link_Point(Right_V[0],X_Border_Max,Right_V[1],Y_Border_Max,r_border);//线下连钱，成功使出环岛
        // }
        Left_Center=1;
        if(HuanDao_Stas==14 && Right_Up_Point==0 && LianXu_Left==0 && LianXu_Right==0 && Right_Vpoint==0)//当拐点消失完使出环岛，，环岛连续出现其他元素可能存在误判，，导致无法完全退出环岛
        {
            Left_Center=0;
            HuanDao_Stas=0;//右环岛结束
            HuanDao_Flag=0;
        }
    }
}
//上拐点单边入环    
void Judge_HuanDao()
{
    Right_Center=0;
    Left_Center=0;
    //环岛判断
    //不在其他元素中，，并且左边或右边丢线
    //下拐点的生长方向比较稳定，，连续断裂点也很稳定但未使用断裂点进行连线
    //丢线数需要测量（所有的丢线数）
    //近距离的圆弧拐点识别条件严苛，，经常识别不到
    //环岛的最长白列很长（可以减少很多误判）（未使用）
    if(HuanDao_Stas==0 && ShiZi_Flag2==0 )//环岛判断的前置条件，，不满足这写条件是不会进行环岛判断的,,增加判断条件（左右丢线数不一样）
    {
        //两个圆弧拐点并不一定要满足（可以去掉）（未经过测量）
        if(L_Up_Turn_Left_Point_Flag_1==1 && R_Up_Turn_Right_Point_Flag_1==0 && L_Border_Point_Num>15 && R_Border_Point_Num<5 && Right_ZhiDao_Flag==1)//左右下拐点单独出现，，可以进行圆弧拐点识别
        {
            //一维数组进行圆弧判断Left_Monotonicity = Left_Monotonicity_Change(L_Up_Turn_Left_Point_1[1]-10,hightest+10,Longest_Left_Line);//检测左侧圆弧拐点  小环岛
            Left_Monotonicity = Left_Monotonicity_Change(L_Up_Turn_Left_Point_1[1]-10,hightest,Longest_Left_Line);//检测左侧圆弧拐点  大环岛
            if(HuanDao_Stas==0 && LianXu_Right==0 && LianXu_Left!=0 && //左不连续，，右连续
                L_Up_Turn_Left_Point_Flag_1==1 &&  Left_Monotonicity!=0 && (Left_Monotonicity-L_Up_Turn_Left_Point_1[1]<-10))//判断为左环岛
            {
                HuanDao_Flag=1;//左环岛
                HuanDao_Stas=1;//进入左环岛状态机
            }else 
            {
                HuanDao_Flag=0;//环岛标志位清零
            }
        }
        else if(R_Up_Turn_Right_Point_Flag_1==1 && L_Up_Turn_Left_Point_Flag_1==0 && R_Border_Point_Num>15 && L_Border_Point_Num<10 && Left_ZhiDao_Flag==1)//左右下拐点单独出现，
        {
            //Right_Monotonicity=Right_Monotonicity_Change(R_Up_Turn_Right_Point_1[1]-10,hightest,Longest_Right_Line);//检测右侧圆弧拐点 小环岛
            Right_Monotonicity=Right_Monotonicity_Change(R_Up_Turn_Right_Point_1[1]-10,hightest,Longest_Right_Line);//检测右侧圆弧拐点 大环岛
            if(HuanDao_Stas==0 && LianXu_Right!=0 && LianXu_Left==0 &&//左连续，，右不连续
                R_Up_Turn_Right_Point_Flag_1==1 &&  Right_Monotonicity!=0 && (Right_Monotonicity-R_Up_Turn_Right_Point_1[1]<-10))//判断为右环岛
            
            {
                HuanDao_Flag=2;//右环岛
                HuanDao_Stas=8;//进入右环岛状态机
            }
            else 
            {
                HuanDao_Flag=0;//环岛标志位清零
            }
        }
        else
        {
             HuanDao_Flag=0;//环岛标志位清零
        }
    }
   //左环岛状态机
   if(HuanDao_Stas==1)//初见环岛，，将圆弧拐点与下拐点连线连线，
   {
       //圆弧拐点误判为上转右拐点，，（难办）
       Left_Monotonicity = Left_Monotonicity_Change(L_Up_Turn_Left_Point_1[1]-10,hightest,Longest_Left_Line);//检测左侧圆弧拐点
       if(Left_Monotonicity!=0 &&   L_Up_Turn_Left_Point_Flag_1==1 &&  L_Up_Turn_Left_Point_Position_1<100 && //出现圆弧拐点
           (L_Up_Turn_Left_Point_1[1]>Left_Monotonicity))//下拐点和圆弧拐点符合环岛位置，，进行连线
       {
           A_Point2=L_Up_Turn_Left_Point_1[1];//用来检测左下拐点消失，，记录的时满足环岛状态1的下拐点
           Right_Center=1;//单边巡线
           // Link_Point(l_border[Left_Monotonicity],L_Up_Turn_Left_Point_1[0],Left_Monotonicity,L_Up_Turn_Left_Point_1[1],l_border);//连线函数
       } //环岛状态机进行跳变的条件，，当左下角拐点消失（同时一维数组的圆弧拐点同时消失）此圆弧拐点任务结束//检验上转左拐点位置，尝试修改一下判断条件
       else if(HuanDao_Stas==1 && abs(A_Point2-L_Up_Turn_Left_Point_1[1])>30 && LianXu_Right==0)
       {
           //当下拐点跳跃时说明左下拐点已经消失
           HuanDao_Stas=2;
           Right_Center=0;
       }
   }
   else if(HuanDao_Stas==2)//初入环岛，，左下角拐点消失，将圆弧拐点与图像左下角连接
   {
        Clear_Gyroscope_Angle();
       //限制圆弧识别范围，，减少误判
       if(L_Right_Turn_Up_Point_Flag_1==1 && R_Left_Turn_Up_Point_Position_1>100)
       {
           Left_Monotonicity = Left_Monotonicity_Change(85,L_Right_Turn_Up_Point_1[1]+6,l_border);//检测左侧圆弧拐点
       }
       else
       {
           Left_Monotonicity = Left_Monotonicity_Change(85,hightest+10,l_border);//检测左侧圆弧拐点
       }
       //对于下拐点的判断生长方向判断更加准确，，一维数组的撕裂点的判断条件右待优化
       if( Left_Monotonicity!=0)//识别到圆弧拐点
       {
         Right_Center=1;//单边巡线
        //    Link_Point(l_border[Left_Monotonicity],X_Border_Min,Left_Monotonicity,Y_Border_Max,l_border);//连线函数
       }
       else if(HuanDao_Stas==2 && Left_Monotonicity==0 && LianXu_Right==0 && Left_Vpoint!=0)//当二维数组圆弧拐点消失，，提前进入下一个状态提前打角，防止撤回一个环岛
       {
           HuanDao_Stas=3;//进入状态3
       }
   }
   else if(HuanDao_Stas==3)//连接V点进行补线，，，进入环岛,,将右边线封死
   {
       //一维数组上拐点位置可能不在V点上（尤其距离V点越近偏移越严重）
       //可以尝试使用上拐点（生长方向进行连线但是生长方向的点智能作为辅助，因为生长方向经常识别不到V点，导致无法进入环岛）
       //无法完全避免V点偏移除非增减识别V点的函数（Y坐标大与周围点的点视为V点）只有在近距离才能检测到
       //进环岛的右边线可以增大斜率，，保证速度快的时候也可以进入环岛
       // Left_Monotonicity = Left_Monotonicity_Change(115,hightest);//检测左侧圆弧拐点
       if(L_Right_Turn_Up_Point_Flag_1==1)//使用两种拐点进行连线，，增加稳定性
       {
           Link_Point(L_Right_Turn_Up_Point_1[0],X_Border_Max,L_Right_Turn_Up_Point_1[1],(Y_Border_Max),r_border);//连线函数
           Link_Point(L_Right_Turn_Up_Point_1[0],(X_Border_Min),L_Right_Turn_Up_Point_1[1],hightest,r_border);//连线函数
       }
       else if(Left_Vpoint!=0)
       {
           Link_Point(Left_V[0],X_Border_Max,Left_V[1],Y_Border_Max,r_border);//连线函数
           Link_Point(Left_V[0],(X_Border_Min),Left_V[1],hightest,r_border);//连线函数
       } 
       else if(Left_Up_Point!=0)
       {
           Link_Point(l_border[Left_Up_Point],X_Border_Max,Left_Up_Point,Y_Border_Max,r_border);//连线函数
           Link_Point(l_border[Left_Up_Point],(X_Border_Min),Left_Up_Point,hightest,r_border);//连线函数
       }
        // Left_Center=1;
       //一维数组上拐点消失，，或这生长方向的拐点消失，，组合判断V点 && Left_Up_Point==0
       if(HuanDao_Stas==3  && Left_Vpoint==0 && L_Right_Turn_Up_Point_Flag_1==0 && FJ_Angle<=-30)
       {
           HuanDao_Stas=4;//进入状态4
       }
   }
   else if(HuanDao_Stas==4)//V点从左下边线变到右边线
   {
       if(R_Left_Turn_Up_Point_Flag_1==1)
       {
           Link_Point(R_Left_Turn_Up_Point_1[0],X_Border_Max,R_Left_Turn_Up_Point_1[1],(Y_Border_Max),r_border);//连线函数
           Link_Point(R_Left_Turn_Up_Point_1[0],(X_Border_Min),R_Left_Turn_Up_Point_1[1],(hightest),r_border);//连线函数
       }
       else if(Right_Vpoint!=0)
       {
           Link_Point(Right_V[0],X_Border_Max,Right_V[1],(Y_Border_Max),r_border);//连线函数
           Link_Point(Right_V[0],(X_Border_Min),Right_V[1],(hightest),r_border);//连线函数
       }
       else if(Right_Up_Point!=0)
       {
           Link_Point(r_border[Right_Up_Point],X_Border_Max,Right_Up_Point,(Y_Border_Max),r_border);//连线函数
           Link_Point(r_border[Right_Up_Point],(X_Border_Min),Right_Up_Point,(hightest),r_border);//连线函数
       }
    //    Left_Center=1;
       //组合判断V点
       if(HuanDao_Stas==4 && R_Left_Turn_Up_Point_Flag_1==0 && Right_Vpoint==0)
       {
           HuanDao_Stas=5;//进入状态5
       }
   }
   else if(HuanDao_Stas==5)//成功进入环岛
   {
       //单边巡线
    //    Left_Center=1;
       //正常巡线，，寻找出环岛的点
       if(HuanDao_Stas==5 && Right_AApoint==1)
       {
           HuanDao_Stas=6;//进入状态6
       }
   }
   else if(HuanDao_Stas==6)//出环岛
   {
    //    出环岛也将右边线封死，，至到出现左上拐点或则右边线连续
       if(Right_AApoint==1)
       {
           //记录最后一个拐点
           A_Point=Right_AA[1];
           Link_Point(Right_AA[0],(X_Border_Min),Right_AA[1],hightest,r_border);//连线函数
       }
       else if(Right_AApoint==0)
       {
           //用记录的拐点补线
           Link_Point((r_border[A_Point]),(X_Border_Min),(A_Point),(hightest),r_border);//连线函数
       }
    //    Left_Center=1;&& Left_Up_Point>=10
       if(HuanDao_Stas==6  && Right_ZhiDao_Flag==1)//寻找到左上拐点时进入状态7或者当右边线连续的时后跳转到下一个状态
       {
           HuanDao_Stas=7;
       }
   }
   else if(HuanDao_Stas==7)//使出环岛
   { 
        Right_Center=1;
       if(HuanDao_Stas==7 && Right_ZhiDao_Flag==1 && Left_ZhiDao_Flag==1)
       {
           HuanDao_Stas=0;//，左环岛结束
           HuanDao_Flag=0;
           Right_Center=0;
       }
   }
   //右环岛状态机
   if(HuanDao_Stas==8)//初见环岛
   {
       Right_Monotonicity=Right_Monotonicity_Change(R_Up_Turn_Right_Point_1[1]-10,hightest,Longest_Right_Line);//检测右侧圆弧拐点
       if(Right_Monotonicity!=0 && R_Up_Turn_Right_Point_Flag_1==1 && R_Up_Turn_Right_Point_Position_1<100 &&//右边圆弧出现，，下拐点出现
           (Right_Monotonicity<R_Up_Turn_Right_Point_1[1]))//下拐点出现在圆弧下边
       {
           A_Point2=R_Up_Turn_Right_Point_1[1];//用来检测左下拐点消失，，记录的时满足环岛状态8的下拐点
           Left_Center=1;
           // Link_Point(r_border[Right_Monotonicity],R_Up_Turn_Right_Point_1[0],Right_Monotonicity,R_Up_Turn_Right_Point_1[1],r_border);//连线函数
       }
       else if(HuanDao_Stas==8 && abs(A_Point2-R_Up_Turn_Right_Point_1[1])>30 && LianXu_Left==0)//下拐点突变，，左边连续
       {
           //当下拐点跳跃时说明左下拐点已经消失
           HuanDao_Stas=9;
           Left_Center=0;
       }
   }
    else if(HuanDao_Stas==9)//初入环岛
   {
        Clear_Gyroscope_Angle();
       //圆弧拐点的位置选取
       if(R_Left_Turn_Up_Point_Flag_1==1 && R_Left_Turn_Up_Point_Position_1>100)
       {
           Right_Monotonicity = Right_Monotonicity_Change(85,R_Left_Turn_Up_Point_1[1]+8,r_border);//检测右侧圆弧拐点
       }
       else
       {
           Right_Monotonicity = Right_Monotonicity_Change(85,hightest+10,r_border);//检测右侧圆弧拐点
       }
       if( Right_Monotonicity!=0)//识别到圆弧拐点
       {
            Left_Center=1;
        //    Link_Point(r_border[Right_Monotonicity],X_Border_Max,Right_Monotonicity,Y_Border_Max,r_border);//连线函数
       }
       //陀螺仪航向角清零
       else if(HuanDao_Stas==9 && LianXu_Left==0 && Right_Vpoint!=0)//当二维数组圆弧拐点消失，上拐点出现在某一位置时，提前进入下一个状态提前打角，防止撤回一个环岛
       {
           HuanDao_Stas=10;//进入状态10
       }
   }
   else if(HuanDao_Stas==10)//右边线出现V点。。链接V点到左下位置
   {
       //进入环岛的链接点可以链接到扫线最高的那一行hightest
       //一维数组上拐点位置可能不在V点上（尤其距离V点越近偏移越严重）
       //可以尝试使用上拐点（生长方向进行连线但是生长方向的点智能作为辅助，因为生长方向经常识别不到V点，导致无法进入环岛）
       //无法完全避免V点偏移除非增减识别V点的函数（Y坐标大与周围点的点视为V点）只有在近距离才能检测到
       //进环岛的右边线可以增大斜率，，保证速度快的时候也可以进入环岛
       // Right_Monotonicity = Right_Monotonicity_Change(115,hightest);//检测右侧圆弧拐点
       if(R_Left_Turn_Up_Point_Flag_1==1)//使用两种拐点进行连线，，增加稳定性
       {
           Link_Point(R_Left_Turn_Up_Point_1[0],X_Border_Min,R_Left_Turn_Up_Point_1[1],(Y_Border_Max),l_border);//连线函数，向下连
           Link_Point(R_Left_Turn_Up_Point_1[0],(X_Border_Max),R_Left_Turn_Up_Point_1[1],hightest,l_border);//连线函数，向上连
       }
       else if(Right_Vpoint!=0)//当左转上拐点不稳定时补充右上拐点连线保证进入环岛
       {
           Link_Point(Right_V[0],X_Border_Min,Right_V[1],Y_Border_Max,l_border);//连线函数
           Link_Point(Right_V[0],(X_Border_Max),Right_V[1],hightest,l_border);//连线函数
       }
       else if(Right_Up_Point!=0)//当左转上拐点不稳定时补充右上拐点连线保证进入环岛
       {
           Link_Point(r_border[Right_Up_Point],X_Border_Min,Right_Up_Point,Y_Border_Max,l_border);//连线函数
           Link_Point(r_border[Right_Up_Point],(X_Border_Max),Right_Up_Point,hightest,l_border);//连线函数
       }
        // Right_Center=1;
       //一维数组上拐点消失，，或这生长方向的拐点消失
       if(HuanDao_Stas==10 && Right_Vpoint==0 && R_Left_Turn_Up_Point_Flag_1==0 && FJ_Angle>=30)
       {
           HuanDao_Stas=11;//进入状态4
       }
   }
   else if(HuanDao_Stas==11)//V点跑到左边线，，这个状态对环岛的影响比较小
   {
       if(L_Right_Turn_Up_Point_Flag_1==1)//生长方向识别到的V点，，连接左下角
       {
           Link_Point(L_Right_Turn_Up_Point_1[0],X_Border_Min,L_Right_Turn_Up_Point_1[1],(Y_Border_Max),l_border);//连线函数
           Link_Point(L_Right_Turn_Up_Point_1[0],(X_Border_Max),L_Right_Turn_Up_Point_1[1],(hightest),l_border);//连线函数
       }
       else if(Left_Vpoint==1)
       {
           Link_Point(Left_V[0],X_Border_Min,Left_V[1],Y_Border_Max,l_border);//连线函数
           Link_Point(Left_V[0],(X_Border_Max),Left_V[1],hightest,l_border);//连线函数
       }
       else if(Left_Up_Point!=0)//当生长方向识别不稳定时，，补充左拐点进行补线，，左拐点位置不准确但一定会出现，，可以保证进入环岛
       {
           Link_Point(l_border[Left_Up_Point],X_Border_Min,Left_Up_Point,(Y_Border_Max),l_border);//连线函数
           Link_Point(l_border[Left_Up_Point],(X_Border_Max),Left_Up_Point,(hightest),l_border);//连线函数，，连接到hightest确保驶入环岛
       }
    //    Right_Center=1;
       if(HuanDao_Stas==11 && Left_Vpoint==0 && L_Right_Turn_Up_Point_Flag_1==0)//V点从左边线消失
       {
           HuanDao_Stas=12;//进入状态12
       }
   }
   else if(HuanDao_Stas==12)//进入环岛中，正常巡线
   {
        // Right_Center=1;
        ////////////////////////////////////////////////////
        // CAR_RUN=0;//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
        ////////////////////////////////////////////////////////
       //正常巡线，，寻找出环岛的点
       if(HuanDao_Stas==12 && Left_AApoint==1)
       {
           HuanDao_Stas=13;//进入状态13
       }
   }
   else if(HuanDao_Stas==13)//出环岛，，还有优化空间
   {
       //出环岛也将左边线封死，，至到出现右上拐点或则左边线连续
       if(Left_AApoint==1)
       {
           //记录最后一个拐点
           A_Point=Left_AA[1];
           Link_Point(Left_AA[0],X_Border_Max,Left_AA[1],hightest,l_border);//连线函数，，连到搜素最高的y值
       }
       else if(R_Up_Turn_Right_Point_Flag_1==0)
       {
           //用记录的拐点补线
           Link_Point((l_border[A_Point]),X_Border_Max,(A_Point),hightest,l_border);//连线函数
       }
        // Right_Center=1;
       //Right_Up_Point 替换为 VPoint(待测试)&& Right_Up_Point>=10
       if(HuanDao_Stas==13 && LianXu_Left==0 && Left_ZhiDao_Flag==1)//寻找到左上拐点时进入状态7或者当右边线连续的时后跳转到下一个状态
       {
           HuanDao_Stas=14;
       }
   }
   else if (HuanDao_Stas==14)//识别右上拐点，，连线，直道使出环岛
   {
       Left_Center=1;
       if(HuanDao_Stas==14 && Right_ZhiDao_Flag==1 && Left_ZhiDao_Flag==1)//当拐点消失完使出环岛，，环岛连续出现其他元素可能存在误判，，导致无法完全退出环岛
       {
           HuanDao_Stas=0;//右环岛结束
           HuanDao_Flag=0;
           Left_Center=0;
       }
   }
}
//单边失败板环岛
void Judge_HuanDao3()
{
    Right_Center=0;
    Left_Center=0;
    if(HuanDao_Stas==0 && ShiZi_Flag2==0)//环岛判断的前置条件，，不满足这写条件是不会进行环岛判断的,,增加判断条件（左右丢线数不一样）
    {
        //两个圆弧拐点并不一定要满足（可以去掉）（未经过测量）
        if(L_Up_Turn_Left_Point_Flag_1==1 && R_Up_Turn_Right_Point_Flag_1==0 && L_Border_Point_Num>15 && R_Border_Point_Num<10)//左右下拐点单独出现，，可以进行圆弧拐点识别
        {
            //一维数组进行圆弧判断
            Left_Monotonicity = Left_Monotonicity_Change(L_Up_Turn_Left_Point_1[1]-10,hightest+10,Longest_Left_Line);//检测左侧圆弧拐点
            if(HuanDao_Stas==0 && LianXu_Right==0 && LianXu_Left!=0 && //左不连续，，右连续
                L_Up_Turn_Left_Point_Flag_1==1 &&  Left_Monotonicity!=0 && (Left_Monotonicity-L_Up_Turn_Left_Point_1[1]<-10))//判断为左环岛
            {
                HuanDao_Flag=1;//左环岛
                HuanDao_Stas=1;//进入左环岛状态机
            }else 
            {
                HuanDao_Flag=0;//环岛标志位清零
            }
        }
        else if(R_Up_Turn_Right_Point_Flag_1==1 && L_Up_Turn_Left_Point_Flag_1==0 && R_Border_Point_Num>15 && L_Border_Point_Num<10)//左右下拐点单独出现，
        {
            Right_Monotonicity=Right_Monotonicity_Change(R_Up_Turn_Right_Point_1[1]-10,hightest+10,Longest_Right_Line);//检测右侧圆弧拐点
            if(HuanDao_Stas==0 && LianXu_Right!=0 && LianXu_Left==0 &&//左连续，，右不连续
                R_Up_Turn_Right_Point_Flag_1==1 &&  Right_Monotonicity!=0 && (Right_Monotonicity-R_Up_Turn_Right_Point_1[1]<-10)//判断为右环岛
            )
            {
                HuanDao_Flag=2;//右环岛
                HuanDao_Stas=8;//进入右环岛状态机
            }
            else 
            {
                HuanDao_Flag=0;//环岛标志位清零
            }
        }
        else
        {
            HuanDao_Flag=0;//环岛标志位清零
        }
    }//左环岛状态机
    if(HuanDao_Stas==1)
    {
        Right_Center=1;
        if(L_Right_Turn_Up_Point_Flag_1==1 && R_Left_Turn_Up_Point_Position_1>100)
        {
            Left_Monotonicity = Left_Monotonicity_Change(65,L_Right_Turn_Up_Point_1[1]+6,l_border);//检测左侧圆弧拐点
        }
        else
        {
            Left_Monotonicity = Left_Monotonicity_Change(65,hightest+10,l_border);//检测左侧圆弧拐点
        }
        if(HuanDao_Stas==1 && Left_Vpoint==1 && LianXu_Right==0)
        {
            HuanDao_Stas=2;
            Right_Center=0;
        }
    }
    else if(HuanDao_Stas==2)
    {
        if(Left_Vpoint==1 || L_Right_Turn_Up_Point_Flag_1==1)
        {
            Left_Center=1;
        }
        else if(Right_Vpoint==1 || R_Left_Turn_Up_Point_Flag_1==1)
        {
            Left_Center=1;
        }
        else if(HuanDao_Stas==2 && Left_Vpoint==0 && L_Right_Turn_Up_Point_Flag_1==0 && Right_Vpoint==0 && R_Left_Turn_Up_Point_Flag_1==0)
        {
            Left_Center=0;
            HuanDao_Stas=3;
        }
    }
    else if(HuanDao_Stas==3)
    {
        if(HuanDao_Stas==3 && Right_AApoint==1)
        {
            HuanDao_Stas=4;
        }
    }
    else if(HuanDao_Stas==4)
    {
        if(Right_AApoint==1)
        {
            //记录最后一个拐点
            A_Point=Right_AA[1];
            Link_Point(Right_AA[0],(X_Border_Min),Right_AA[1],hightest,r_border);//连线函数
        }
        else if(Right_AApoint==0)
        {
            //用记录的拐点补线
            Link_Point((r_border[A_Point]),(X_Border_Min),(A_Point),(hightest),r_border);//连线函数
        }
        // Add_Roundabout_Line(0.9,X_Border_Min,hightest,Y_Border_Max,r_border);
        // Link_Point(X_Border_Min,X_Border_Max,hightest,Y_Border_Max,r_border);
        if(HuanDao_Stas==4 && L_Right_Turn_Up_Point_Flag_1==1 && LianXu_Right==0 && Left_Up_Point>=20 && Right_V[1]>=20 && LianXu_Left!=0)//寻找到左上拐点时进入状态7或者当右边线连续的时后跳转到下一个状态
        {
            HuanDao_Stas=5;
        }
    }
    else if(HuanDao_Stas==5)
    {
        Right_Center=1;
        if(HuanDao_Stas==5 && Left_Up_Point==0 && LianXu_Right==0 && LianXu_Left==0)
        {
           HuanDao_Stas=0;//，左环岛结束
           HuanDao_Flag=0;
           Right_Center=0;
        }
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     障碍物判断函数
  @param     三重判断
  @return    无
  Sample     Judge_BiZhang()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
//避障判断
int BiZhang_Flag=0;
int Last_BiZhang_Flag=0;
int BiZhang_Biaozhi=0;
int BiZhang_Image=0;
int BiZhang_Image2=0;
int Black_Flag =0;
int BiZhang_Stace=0;
float Bizhang_Num=0;
uint16 cha1=0,cha2=0,cha3=0;
void Judge_BiZhang()
{
    int i=0;
    // int Jisuan_Bizhang=0;
    // BiZhang_Flag=0;
    BiZhang_Stace=0;
    Bizhang_Num=0;
    uint8 enough_sum=0;
    uint8 kuandu=0;
    uint8 last_kuandu=0;
    uint8 basic_point=0;
    cha1=0;
    cha2=0;
    cha3=0;
    //识别边线断裂
    if(L_Border_Point_Num<5 && R_Border_Point_Num<5 && Right_ZhiDao_Flag==1 && hightest<10)//判断时候双边不丢线
    {
        //当赛道中线出现断裂，进行障碍物判断
        for(i=start_point_l[1];i>hightest+10;i--)
        {
            cha1=center_line[i-3]-center_line[i];
            cha2=center_line[i-3]-center_line[i+1];
            cha3=center_line[i-3]-center_line[i+2];
            if(cha1>=2&&abs(cha3-cha1)<=2&&abs(cha2-cha3)<=2)//中线撕裂程度判断
            {
                Bizhang_Num=1;
                break;
            }
        }
        // Bizhang_Num = Center_Jidge(center_line);
        if(Bizhang_Num==1)//判断左右边线是否断裂
        {
            for (i = 4; i < (data_stastics_l - 4); i++) 
            {
                cha1=points_l[i-3][1]-points_l[i][1];
                cha2=points_l[i-3][1]-points_l[i+1][1];
                cha3=points_l[i-3][1]-points_l[i+2][1];
                if(cha1>=2&&abs(cha3-cha1)<=2&&abs(cha2-cha3)<=2)//边线撕裂判断
                {
                    BiZhang_Stace=1;
                    break;
                }
            }
        }
        if(BiZhang_Stace==1)
        {
            for(i=start_point_l[1];i>hightest+10;i--)
            {
                kuandu= r_border[i]-l_border[i];
                if(kuandu<=last_kuandu-15 && basic_point==0)
                {
                    basic_point=last_kuandu;
                }
                if(basic_point!=0)
                {
                    if(kuandu<=basic_point-15)
                    {
                        enough_sum++;
                    }
                }
                if(enough_sum>10)
                {
                    if(LianXu_Left>LianXu_Right)
                    {
                        BiZhang_Flag=2;
                        BiZhang_Stace=0;
                    }
                }
                last_kuandu=kuandu;
            }
        }
    }
    if(L_Border_Point_Num<5 && R_Border_Point_Num<5 && Left_ZhiDao_Flag==1 && hightest<10)//判断时候双边不丢线
    {
        //当赛道中线出现断裂，进行障碍物判断
        for(i=start_point_l[1];i>hightest+10;i--)
        {
            cha1=center_line[i-3]-center_line[i];
            cha2=center_line[i-3]-center_line[i+1];
            cha3=center_line[i-3]-center_line[i+2];
            if(cha1>=2&&abs(cha3-cha1)<=2&&abs(cha2-cha3)<=2)//中线撕裂程度判断
            {
                Bizhang_Num=2;
                break;
            }
        }
        if(Bizhang_Num==2)
        {
            for (i = 4; i < (data_stastics_r - 4); i++) 
            {
                cha1=points_r[i-3][1]-points_r[i][1];
                cha2=points_r[i-3][1]-points_r[i+1][1];
                cha3=points_r[i-3][1]-points_r[i+2][1];
                if(cha1>=2&&abs(cha3-cha1)<=2&&abs(cha2-cha3)<=2)//判断左右边线是否断裂
                {
                    BiZhang_Stace=2;
                    break;
                }
            }
        }
        if(BiZhang_Stace==2)
        {
            for(i=start_point_l[1];i>hightest+10;i--)
            {
                kuandu= r_border[i]-l_border[i];
                if(kuandu<=last_kuandu-15 && basic_point==0)
                {
                    basic_point=last_kuandu;
                }
                if(basic_point!=0)
                {
                    if(kuandu<=basic_point-15)
                    {
                        enough_sum++;
                    }
                }
                if(enough_sum>10)
                {
                    if(LianXu_Left<LianXu_Right)
                    {
                        BiZhang_Flag=1;
                        BiZhang_Stace=0;
                    }
                }
                last_kuandu=kuandu;
            }
        }
    }
    int Compare_Bizhng_Time=15;
    if(Speed_Target<=450)
    {
        Compare_Bizhng_Time=20;
    }
    else
    {
        Compare_Bizhng_Time=15;
    }
    // printf("%d\n",BiZhang_Stace);
    if(BiZhang_Flag==1)
    {
        BiZhang_Image++;
        if(BiZhang_Image>=Compare_Bizhng_Time)
        {
            BiZhang_Flag=0;
            BiZhang_Image=0;
        }
    }
    if(BiZhang_Flag==2)
    {
        BiZhang_Image2++;
        if(BiZhang_Image2>=Compare_Bizhng_Time)
        {
            BiZhang_Flag=0;
            BiZhang_Image2=0;
        }
    }
    // Last_BiZhang_Flag=BiZhang_Flag;
}
//坡道判断
int PoDao_Flag=0;
int Last_Pitch=0;
/*-------------------------------------------------------------------------------------------------------------------
  @brief     坡道函数
  @param     陀螺仪积分突变
  @return    无
  Sample     Judge_PoDao()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
void Judge_PoDao()//使用陀螺仪积分
{
    //检测陀螺仪突变
    PoDao_Flag=0;
    if(HuanDao_Stas==0)
    {
        if(abs(FJ_Pitch-Last_Pitch)>4)
        {
            PoDao_Flag=1;
        }
    }
    Last_Pitch=FJ_Pitch;
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     坡道函数
  @param     摄像头赛道变宽
  @return    无
  Sample     Judge_PoDao2()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
void Judge_PoDao2()
{
    int i=0;
    int Jisuan_PoDao=0;
    PoDao_Flag=0;
    if(hightest<=2)
    {
        if(BiZhang_Flag==0 && ShiZi_Flag2==0 && WanDao_Flag==0 && HuanDao_Stas==0)
        {
            for(i=start_point_l[1];i>hightest;i--)
            {
                if((Longest_Right_Line[i]-Longest_Left_Line[i])-Zhi_Dao_BuChang[i]>10)
                {
                    Jisuan_PoDao++;
                }
            }
        }
    }
    if(Jisuan_PoDao>=35)
    {
        PoDao_Flag=1;
    }
}
/*-------------------------------------------------------------------------------------------------------------------
  @brief     斑马线判断函数
  @param     长白列突变
  @return    无
  Sample     Judge_BanMaXian()
  @note      参数内部修改
-------------------------------------------------------------------------------------------------------------------*/
int BanMaXianTime=0;
int BanMa_Flag=0;//斑马线标志
void Judge_BanMaXian()
{
    int i=0;
    int count=0;
    BanMa_Flag=0;
    if(CAR_RUN==1 && BanMaXianTime<60)//限制斑马线计数大小
    {
        BanMaXianTime++;
    }
    else if (CAR_RUN==0)
    {
        BanMaXianTime=0;//停车清零
    }
    if(BanMaXianTime>50)//斑马线计数大于一定值后开始识别斑马线
    {
        if(R_Border_Point_Num<20 && L_Border_Point_Num<20)
        {
            for(i=15;i<image_w-15;i++)
            {
                if(abs(Wite_Line[i]-Wite_Line[i+1])>30)//计算最长白列的差值，大于8判断为环岛
                {
                    count++;
                }
            }
        }
        if(count>=8)
        {
            BanMa_Flag=1;
        }
    }
}
int Huandao_Key=1;
int Shizi_Key=1;
int Zhidao_Key=1;
int Banma_Key=1;
int Podao_Key=1;
int BiZhang_Key=1;
int WanDao_Key=1;
int Car_Stop_Flag=0;
int Car_stop_Image=0;
int Podao_Num=0;
int Podao_Image=0;
int Bizhang_P=0;
int Bizhang_P_Image=0;
//各个元素优先级判断
void YuanSu_Gade()
{
    if(Zhidao_Key==1)
    {
        //直道判断
        Judge_ZhiDao1();
    }
    //环岛元素处理  && CAR_RUN==1
    if(Huandao_Key==1 && CAR_RUN==1)//发车后进行环岛判断
    {
        //环岛状态
        Judge_HuanDao();
        //环岛退出
        HuanDao_Break();
    }
    else if(Huandao_Key==1 && CAR_RUN==0)//环岛内意外停车，直接退出换到状态机
    {
        HuanDao_Stas=0;
    }
    if(Shizi_Key==1)
    {
        //十字元素处理
        Judge_ShiZi();
    }
    if(WanDao_Key==1)
    {
        //弯道判断
        Judge_WanDao();
    }
    //坡道延时处理，延长坡道识别时间
    if(Podao_Key==1)
    {
        //坡道
        Judge_PoDao();
        if(PoDao_Flag==1)
        {
            Podao_Num=1;
        }
    }
    //坡道缓冲，延长坡道识别时间
    if(Podao_Num==1)
    {
        Podao_Image++;
        PoDao_Flag=1;
        if(Podao_Image>10)
        {
            PoDao_Flag=0;
            Podao_Image=0;
            Podao_Num=0;
        }
    }
    if(Banma_Key==1)
    {
        //斑马线
        Judge_BanMaXian();
    }
    if(BiZhang_Key==1)
    {
        //障碍物&& BanMaXianTime>50
        if(HuanDao_Stas==0 && PoDao_Flag==0 && WanDao_Flag==0 && BanMaXianTime>50 && BanMa_Flag==0)
        {
            Judge_BiZhang();
        }
        else if ((HuanDao_Stas!=0 || PoDao_Flag!=0 || WanDao_Flag!=0 || BanMa_Flag!=0) && BiZhang_Flag==1)
        {
            BiZhang_Flag=0;
        }
    }
    if(Last_BiZhang_Flag!=BiZhang_Flag && BiZhang_Flag==0)
    {
        Bizhang_P=1;
    }
    if(Bizhang_P==1)
    {
        Bizhang_P_Image++;
        if(Bizhang_P_Image>10 || HuanDao_Stas!=0 || PoDao_Flag!=0 || WanDao_Flag!=0 || BanMa_Flag!=0)
        {
            Bizhang_P_Image=0;
            Bizhang_P=0;
        }
    }
    Last_BiZhang_Flag=BiZhang_Flag;
    //停车延时处理，识别到斑马线超过一定时间后停车
    if(BanMa_Flag==1)
    {
        // CAR_RUN=0;
        // FuYa_Key=0;
        Car_Stop_Flag=1;
    }
    if(Car_Stop_Flag==1)
    {
        Car_stop_Image++;//停车时间累积
        if(Car_stop_Image>=15)
        {
            CAR_RUN=0;//关闭负压
            FuYa_Key=0;//停车
            //清除参数
            Car_stop_Image=0;
            Car_Stop_Flag=0;
            
        }
    }
    // if(ShiZi_Flag2==1)
    // {
    //     printf("检测到十字\n");
    // }
    // if(BanMa_Flag==1)
    // {
    //     printf("检测到斑马线\n");
    // }
    // if(WanDao_Flag==1)
    // {
    //     printf("检测到弯道\n");
    // }
    // if(PoDao_Flag==1)
    // {
    //     printf("检测到坡道\n");
    // }
    // if(HuanDao_Stas!=0)
    // {
    //     printf("检测到环岛\n");
    // }
    if(BiZhang_Flag==1 && CAR_RUN==1)
    {
        printf("检测到右避障\n");
    }
    if(BiZhang_Flag==2 && CAR_RUN==1)
    {
        printf("检测到左避障\n");
    }
}
//第一版的弯道补偿
uint8_t Wan_Dao_BuChang[90]={
                            0,0,0,0,0,0,0,0,0,0,
                            5,7,8,8,9,9,10,12,13,14,
                            14,16,17,18,19,20,21,22,23,24,
                            25,26,26,27,28,29,30,31,32,33,
                            34,35,36,37,38,39,40,41,42,42,
                            43,44,45,46,47,48,49,50,51,51,
                            53,54,55,56,58,60,61,62,63,63,
                            64,65,66,67,68,69,70,71,72,73,
                            74,75,75,75,75,76,76,77,77,77,
};
int a_Flag=0;
int Right_Center=0;
int Left_Center=0;
int BiZhang_Last_Flag=0;
int BIZhang_Flag2=0;
//获取中线，方便计算误差
void Get_Center_Line(void)
{
    //弯道拟合中线
    // int Level_Point=0;
    // int Buchang_Wandao=0;
    // int j=0;
    int i=0;
    // int Left_JiSuan_Point=0;
    // int Right_JiSuan_Point=0;
    //直道计算中线
    if(Right_Center==1)//右单边
    {
        if(HuanDao_Stas==1 || HuanDao_Stas==2 ||HuanDao_Stas==8 ||HuanDao_Stas==9)//入环单边
        {
            for(i=start_point_l[1];i>hightest;i--)
            {
                center_line[i]=r_border[i]-(uint8)(Zhi_Dao_BuChang[i]/2)-5;
            }
        }
        else if(HuanDao_Stas==12 || HuanDao_Stas==13)//右环岛内单边
        {
            for(i=start_point_l[1];i>hightest;i--)
            {
                center_line[i]=r_border[i]-(uint8)(Zhi_Dao_BuChang[i]/2)+0;
            }
        }
        else//出环岛单边巡线
        {
            for(i=start_point_l[1];i>hightest;i--)
            {
                center_line[i]=r_border[i]-(uint8)(Zhi_Dao_BuChang[i]/2)-5;
            }
        }
    }
    else if (Left_Center==1)//左单边
    {
        if(HuanDao_Stas==1 || HuanDao_Stas==2 ||HuanDao_Stas==8 ||HuanDao_Stas==9)//入环单边
        {
            for(i=start_point_l[1];i>hightest;i--)
            {
                center_line[i]=l_border[i]+(uint8)(Zhi_Dao_BuChang[i]/2)+5;
            }
        }//|| HuanDao_Stas==6
        else if(HuanDao_Stas==5 )//左环岛环内单边
        {
            for(i=start_point_l[1];i>hightest;i--)
            {
                center_line[i]=l_border[i]+(uint8)(Zhi_Dao_BuChang[i]/2)-0;//环岛内切
            }
        }
        else
        {
            for(i=start_point_l[1];i>hightest;i--)//出环岛单边巡线
            {
                center_line[i]=l_border[i]+(uint8)(Zhi_Dao_BuChang[i]/2)+5;
            }
        }
    }
    else//左右边线，正常拟合中线，无多余处理
    {
        for(i=start_point_l[1];i>hightest;i--)
        {
            center_line[i]=(l_border[i]+r_border[i])/2;
        }
    }
    //障碍物中线处理
    if(BiZhang_Flag==1)//右侧障碍物
    {
        for(i=start_point_l[1];i>hightest;i--)
        {
            center_line[i]=l_border[i]+(uint8)(Zhi_Dao_BuChang[i]/2)-15;
        }
    }
    else if(BiZhang_Flag==2)//左侧障碍物
    {
        for(i=start_point_l[1];i>hightest;i--)
        {
            center_line[i]=r_border[i]-(uint8)(Zhi_Dao_BuChang[i]/2)+15;
        }
    }
}
                            // 11,13,15,17,18,19,21,12,15,17,
                            // 19,21,23,25,26,28,30,32,34,36,
                            // 38,40,43,46,48,50,52,54,56,58,
                            // 60,62,64,66,68,70,72,74,77,79,
                            // 82,84,86,88,90,92,94,96,98,99,
                            // 101,103,105,107,109,111,113,115,117,119,
                            // 121,123,126,128,130,132,134,135,137,139    
int Speed_400_Scan_Mun2[80]={
    0,0,0,0,0,0,0,0,0,0,//0~9
    0,0,0,0,0,0,0,0,0,0,//10~19
    0,0,0,0,0,0,88,86,84,82,//20~29
    79,77,74,72,70,68,66,64,62,60,//30~39
    58,56,54,52,50,48,46,43,40,38,//40~49
    1,1,1,1,1,1,1,1,1,1,//50~59
    1,1,1,1,1,1,1,1,1,1,//60~69
    0,0,0,0,0,0,0,0,0,0,//70~79
};
int Speed_450_550_Scan_Mun2[80]={
    0,0,0,0,0,0,0,0,0,0,//0~9
    0,0,0,0,0,0,0,0,0,0,//10~19
    0,0,0,0,92,90,88,86,84,82,//20~29
    79,77,74,72,70,68,66,64,62,60,//30~39
    58,56,54,52,50,48,46,43,40,38,//40~49
    1,1,1,1,1,1,1,1,1,1,//50~59
    1,1,1,1,1,1,1,1,1,1,//60~69
    0,0,0,0,0,0,0,0,0,0,//70~79
};
int Speed_600_Scan_Mun2[80]={
    0,0,0,0,0,0,0,0,0,0,//0~9
    0,0,0,0,0,0,0,0,0,0,//10~19
    0,0,96,94,92,90,88,86,84,82,//20~29
    79,77,74,72,70,68,66,64,62,60,//30~39
    58,56,54,52,50,48,46,43,40,38,//40~49
    1,1,1,1,1,1,1,1,1,1,//50~59
    1,1,1,1,1,1,1,1,1,1,//60~69
    0,0,0,0,0,0,0,0,0,0,//70~79
};
int Scan_Mun2[80];
int Scan_Num[80]={
    0,0,0,0,0,0,0,0,0,0,//0~9
    0,0,0,0,0,0,0,0,0,0,//10~19
    0,0,0,0,0,0,0,0,0,0,//20~29
    1,1,1,1,1,1,1,1,1,1,//30~39
    1,1,1,1,1,1,1,1,1,1,//40~49
    1,1,1,1,1,1,1,1,1,1,//50~59
    1,1,1,1,1,1,1,1,1,1,//60~69
    0,0,0,0,0,0,0,0,0,0,//70~79
                 };//误差权重
//环岛入环前瞻
int HuanDao_Scan_Num[80]={
    0,0,0,0,0,0,0,0,0,0,//0~9
    0,0,0,0,0,0,0,0,0,0,//10~19
    0,0,0,0,0,0,0,0,0,0,//20~29
    1,1,10,10,10,10,10,10,10,10,//30~39
    10,10,7,6,6,5,5,5,4,4,//40~49
    3,3,3,1,1,1,1,1,1,1,//50~59
    1,1,1,1,1,1,1,1,1,1,//60~69
    0,0,0,0,0,0,0,0,0,0,//70~79
};
//环岛前瞻
int HuanDao_Scan_Num2[80]={
                        0,0,0,0,0,0,0,0,0,0,//0~9
                        0,0,0,0,0,0,0,0,0,0,//10~19
                        0,0,0,0,0,0,0,0,0,0,//20~29
                        2,4,4,4,4,4,4,4,4,4,//30~39
                        3,3,3,3,2,2,2,1,1,1,//40~49
                        1,1,1,1,1,1,1,1,1,1,//50~59
                        1,1,1,1,1,1,1,1,1,1,//60~69
                        0,0,0,0,0,0,0,0,0,0,//70~79
};
//动态前瞻
uint8_t hightest_Min = 0;
uint8_t hightest_Max = 0;
uint16_t hightest_Sum = 0;
uint8_t hightest_Average = 0;
uint8_t hightest_Ayery[20] = {0};
int Look_Start_Line=0;
void Scan_Mode(void) {
    uint8_t i = 0;
    // 正确右移数组元素
    for (i = 19; i > 0; i--) 
    {
        hightest_Ayery[i] = hightest_Ayery[i-1];
    }
    hightest_Ayery[0] = hightest;
    // 寻找最小值和最大值
    hightest_Min = hightest_Ayery[0];
    hightest_Max = hightest_Ayery[0];
    for (i = 0; i < 20; i++) 
    {
        if (hightest_Ayery[i] < hightest_Min) 
        {
            hightest_Min = hightest_Ayery[i];
        }
        if (hightest_Ayery[i] > hightest_Max) 
        {
            hightest_Max = hightest_Ayery[i];
        }
    }
    // 计算总和（先清零）
    hightest_Sum = 0;
    for (i = 0; i < 20; i++) 
    {
        hightest_Sum += hightest_Ayery[i];
    }
    hightest_Sum -= (uint16_t)hightest_Min + (uint16_t)hightest_Max;
    // 计算平均值（除以18）
    hightest_Average = (uint8_t)(hightest_Sum / 18);

    hightest_Average = (hightest_Average > 36) ? 36 : (hightest_Average < 1) ? 1 : hightest_Average;

    float Temp_Float = (float)(hightest_Average - 1);     //当直线时，我的Y_Meet值是2

    if(Temp_Float > 35.0f)      //限幅
        {
            Temp_Float = 35.0f;
        }
    if(Temp_Float < 0)
        {
            Temp_Float = 0;
        }
    // 动态前瞻起始位置计算
    //  {10,10,10,10,10,10,10,10,9,9,9};
    uint8_t Get_Scan[20] =  {38,40,43,46,48,50,52,54,56,58,60,62,64,66,68,70,72,74,77,79};//8,8,8,8,8,8,15,20,20,20,20,20,20,15,15,15,10,5,4,4,
    uint8_t Start_Line = (uint8_t)(Temp_Float / 35.0f * 10.0f  + 45.0f);//39
    //保证数组为初始化的数组
    Look_Start_Line=Start_Line;
    memcpy(Scan_Mun2,Scan_Num,sizeof(int)*80);
    // 赋值并检查越界
    if(hightest>=60)//当中线少于10行时从图像最下端进行赋值
    {
        for (i = 0; i < 20; i++) 
        {
            if (Start_Line - i < 70) 
            { 
                Scan_Mun2[70 - i] = Get_Scan[i];
            }
        }
    }
    else//正常情况下的数据赋值
    {
        for (i = 0; i < 20; i++) 
        {
            if (Start_Line - i < 70) 
            { 
                Scan_Mun2[Start_Line - i] = Get_Scan[i];
            }
        }
    }
}
float Mid_Error=0;//最终输出误差
void Get_Center_Error(void)
{
    static float Person_Error=0;//当前误差
    static float Last_Error=0;//上一次误差
    static float Last_Last_Error=0;//上上次误差
	uint8_t i=0;
    float Mid=0;//中线误差和
    int Mid_Num=0;//中线误差和的个数
	Person_Error=0;
    if(Speed_Target<=400)
    {
        memcpy(Scan_Mun2,Speed_400_Scan_Mun2,sizeof(int)*80);
    }
    else if(Speed_Target==450 || Speed_Target==550 || Speed_Target==500)
    {
        memcpy(Scan_Mun2,Speed_450_550_Scan_Mun2,sizeof(int)*80);
    }
    else if(Speed_Target==600)
    {
        memcpy(Scan_Mun2,Speed_600_Scan_Mun2,sizeof(int)*80);
    }
    // Scan_Mode();//动态前瞻
    if(HuanDao_Stas!=0)//环岛前瞻
    {
        for(i=hightest;i<start_point_r[1];i++)
        {
            Mid+=((image_w/2)-center_line[i])*HuanDao_Scan_Num[i];
            Mid_Num+=HuanDao_Scan_Num[i];
        }
    }
    else//其他前瞻
    {
        for(i=hightest;i<start_point_r[1];i++)
        {
            Mid+=((float)(image_w/2)-center_line[i])*Scan_Mun2[i];
            Mid_Num+=Scan_Mun2[i];
        }
    }
	Person_Error=(float)Mid/Mid_Num;
	Last_Last_Error = Last_Error;                                   // 先保存旧的上次误差
    Last_Error = Person_Error;                                      // 再更新当前误差到上次
	Mid_Error=1.0*Person_Error+0.0*Last_Error+0.0*Last_Last_Error;  // 互补滤波
    if(Mid_Error>=45)
    {
        Mid_Error=45;
    }
    else if (Mid_Error<=-45)
    {
        Mid_Error=-45;
    }
}
//在图像内画线
void MyDraw_Line(uint8 *Image)
{
    for(uint8 i=start_point_r[1];i>hightest;i--)
    {
        My_Image[i][Image[i]]=0;//在图像内划线
        My_Image[i][l_border[i]+2]=0;//在图像内划线
        My_Image[i][r_border[i]-2]=0;//在图像内划线
    }
}
int CAR_STOP_St=0;
int Auto_Speed_Flag=0;
void Image_Ses(void)
{
    image_draw_rectan(My_Image);
    data_stastics_l = 0;
    data_stastics_r = 0;
    if (get_start_point(My_Image,image_h - 2))//找到起点了，再执行八领域，没找到就一直找
    {
        CAR_STOP_St=0;
		//八邻域扫线开始
        search_l_r((uint16)(USE_num-2), My_Image, &data_stastics_l, &data_stastics_r, start_point_l[0], start_point_l[1], start_point_r[0], start_point_r[1], &hightest);
		//从二维数组中提取边线
        get_left(data_stastics_l);
	    get_right(data_stastics_r);
        //最长白列寻找边线
        Find_Longest_Wite_Line(My_Image);
        //寻找拐点
		Get_Special_Point();
        //计算方差，，测量赛道的弯曲程度
        // Left_S = S_judge(points_l,data_stastics_l);
        // Right_S= S_judge(points_r,data_stastics_r);
        //计算边线斜率及截距
        // Calculate_Slope_Intercept(hightest,start_point_l[1],l_border,&Left_Slope,&Left_Intercept);
        // Calculate_Slope_Intercept(hightest,start_point_r[1],r_border,&Right_Slope,&Right_Intercept);
        //元素识别
        YuanSu_Gade();
        //计算中线
		Get_Center_Line();
        //速度控制
        if(Auto_Speed_Flag==1)
        {
            Fitting_Speed_Control(1,0,0,0,0,0);
        }
        else
        {
            Fitting_Speed_Control(0,1,0,0,0,0);
        }
		//计算误差
		Get_Center_Error();
        //画中线，和边线
        MyDraw_Line(center_line);
        //舵机角度控制
        Servo_Control();
        if(HuanDao_Stas!=0)
        {
            printf("%d\n",HuanDao_Stas);
        }
    }
    else//当起点丢失时，，停车
    {
        CAR_RUN=0;
        FuYa_Key=0;
        CAR_STOP_St=1;
    }
}
